/*
 * Copyright (C) 2022-23 Texas Instruments Incorporated
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 *   Redistributions of source code must retain the above copyright
 *   notice, this list of conditions and the following disclaimer.
 *
 *   Redistributions in binary form must reproduce the above copyright
 *   notice, this list of conditions and the following disclaimer in the
 *   documentation and/or other materials provided with the
 *   distribution.
 *
 *   Neither the name of Texas Instruments Incorporated nor the names of
 *   its contributors may be used to endorse or promote products derived
 *   from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/**************************************************************************
 *************************** Include Files ********************************
 **************************************************************************/

/* Standard Include Files. */
#include <stdint.h>
#include <stdlib.h>
#include <stddef.h>
#include <string.h>
#include <stdio.h>
#include <math.h>
#include <assert.h>

/* MCU Plus Include Files. */
#include <kernel/dpl/SemaphoreP.h>
#include <kernel/dpl/CacheP.h>
#include <kernel/dpl/ClockP.h>
#include <kernel/dpl/DebugP.h>
#include <kernel/dpl/HwiP.h>
#include <kernel/dpl/AddrTranslateP.h>

#include "FreeRTOS.h"
#include "task.h"

/* mmwave SDK files */
#include <datapath/dpedma/v0/dpedmahwa.h>
#include <datapath/dpedma/v0/dpedma.h>
#include <datapath/dpu/rangeproc/v0/rangeprochwa.h>
#include <datapath/dpu/rangeproc/v0/rangeprochwa_internal.h>
#include <datapath/dpu/mpdproc/v0/mpdproc.h>
#include <datapath/dpu/trackerproc/v0/trackerproc.h>

#include <control/mmwave/mmwave.h>
#include <drivers/edma.h>
#include <drivers/uart.h>
#include <drivers/i2c.h>
#include <drivers/hw_include/cslr_adcbuf.h>

#include <board/ina.h>

#include <utils/mathutils/mathutils.h>
#include "mmw_cli.h"

#include "mmw_res.h"
#include "motion_detect.h"
#include "mmw_flash.h"


#include "ti_drivers_config.h"
#include "ti_drivers_open_close.h"
#include "ti_board_open_close.h"
#include "ti_board_config.h"

// RPMF
#include <FreeRTOS.h>
#include <task.h>
#include <semphr.h>
#include "drivers/power.h"
#include <mmwavelink/include/rl_device.h>
#include <mmwavelink/include/rl_sensor.h>
#include <drivers/prcm.h>
#include <drivers/hw_include/cslr_soc.h>


#define MMWDEMO_RFPARSER_SPEED_OF_LIGHT_IN_METERS_PER_SEC (3e8)
#define EDMA_TEST_EVT_QUEUE_NO      (0U)
#define MAX_NUM_TX_ANTENNA          (2U)
#define MAX_NUM_RX_ANTENNA          (3U)
#define MAX_NUM_RANGEBIN            (64U)
#define MAX_NUM_ADCSAMPLE_PERCHIRP  (128U)
#define MAX_NUM_CHIRPS_PERFRAME     (64U)
#define MAX_AZ_FFT_SIZE             (64U)
#define MAX_NUM_DETECTIONS          (MMWDEMO_OUTPUT_POINT_CLOUD_LIST_MAX_SIZE)
#define DPU_DOAPROC_6432_1432_BOARD 0
#define DPU_DOAPROC_ISK_BOARD 1
#define DPC_OBJDET_DPIF_RADARCUBE_FORMAT_6 6
#define DPC_DPU_DPIF_DETMATRIX_FORMAT_2 2
#define DPC_OBJDET_HWA_WINDOW_RAM_OFFSET 0
#define DPC_DPU_RANGEPROC_FFT_WINDOW_TYPE MATHUTILS_WIN_BLACKMAN
#define DPC_OBJDET_QFORMAT_RANGE_FFT 17
#define MMW_DEMO_TEST_ADC_BUFF_SIZE 1024  //ToDo maximum 128 real samples (int16_t), 3 Rx channels
#define READ_LINE_BUFSIZE   256

#define MMW_DEMO_MAJOR_MODE 0
#define MMW_DEMO_MINOR_MODE 1

#define FRAME_REF_TIMER_CLOCK_MHZ  40

#define RF_SYNTH_TRIM_VALID        (1U)


//Uncomment this for Low power mode verification - bit-matching with uninterrupted power mode
//#define LOW_POWER_DEEP_SLEEP_MODE_VERIFICATION

/* Calibration Data Save/Restore defines */
#define MMWDEMO_CALIB_STORE_MAGIC            (0x7CB28DF9U)

uint8_t gATECalibDataStorage[(ATE_CALIB_DATA_LENGTH + 4)] __attribute__((aligned(8))) = {0};
MmwDemo_calibData gFactoryCalibDataStorage __attribute__((aligned(8))) = {0};

uint8_t                 rxData[16];
uint8_t                 txData[16];
I2C_Transaction         i2cTransaction;


MmwDemo_MSS_MCB gMmwMssMCB = {0};

/*uint32_t gINAPowerWatts = 0;*/

/* Default antenna geometry - XWR6432 EVM */
MmwDemo_antennaGeometryCfg gDefaultAntGeometry = {.ant = {{0,0}, {1,1}, {0,2}, {0,1}, {1,2}, {0,3}}};

/*! @brief     EDMA interrupt objects for DPUs */
Edma_IntrObject     intrObj_rangeProc[2], intrObj_doaProc, intrObj_cfarProc, intrObj_uDopProc;

HwiP_Object gHwiChirpAvailableHwiObject;
HwiP_Object gHwiFrameStartHwiObject;
//Interrupt object for Sensor Stop
HwiP_Object gHwiSensorStopHwiObject;

#if 0
volatile uint32_t gMmwDemoChirpCnt = 0;
volatile uint32_t gMmwDemoChirpStartCnt = 0;
volatile uint32_t gMmwDemoBurstCnt = 0;
#endif

/*! L3 RAM buffer for object detection DPC */
uint8_t gMmwL3[0x40000 + 160*1024]  __attribute((section(".l3")));

/*! Local RAM buffer for object detection DPC */
#define MMWDEMO_OBJDET_CORE_LOCAL_MEM_SIZE ((8U+6U+4U+2U+8U) * 1024U)
uint8_t gMmwCoreLocMem[MMWDEMO_OBJDET_CORE_LOCAL_MEM_SIZE];

/*! Local RAM buffer for tracker */
#define MMWDEMO_OBJDET_CORE_LOCAL_MEM2_SIZE (25U * 1024U)
uint8_t gMmwCoreLocMem2[MMWDEMO_OBJDET_CORE_LOCAL_MEM2_SIZE];


/* User defined heap memory and handle */
#define MMWDEMO_OBJDET_CORE_LOCAL_MEM3_SIZE  (2*1024u)
static uint8_t gMmwCoreLocMem3[MMWDEMO_OBJDET_CORE_LOCAL_MEM3_SIZE] __attribute__((aligned(HeapP_BYTE_ALIGNMENT)));


DPU_DoaProc_HW_Resources  *hwRes;

HWA_Handle hwaHandle;


DPU_RangeProcHWA_Config rangeProcDpuCfg;
DPU_DoaProc_Config doaProcDpuCfg;
DPU_uDopProc_Config uDopProcDpuCfg;
DPU_CFARProcHWA_Config cfarProcDpuCfg;
DPU_MpdProc_Config mpdProcDpuCfg;

typedef struct rangeProcTestConfig_t_ {
    uint32_t numTxAntennas;
    uint32_t numRxAntennas;
    uint32_t numVirtualAntennas;
    uint32_t numAdcSamples;
    uint32_t numRangeBins;
    uint32_t numChirpsPerFrame;
    uint32_t numChirpsPerFrameRef;
    uint32_t numFrames;
} rangeProcTestConfig_t;

extern TaskHandle_t gCliTask;

MMWave_temperatureStats  tempStats;

extern void Mmwave_populateDefaultCalibrationCfg (MMWave_CalibrationCfg* ptrCalibrationCfg);

extern void MMWave_getTemperatureReport (MMWave_temperatureStats* ptrTempStats);

int32_t mmwDemo_registerFrameStartInterrupt(void);
int32_t mmwDemo_registerChirpInterrupt(void);
int32_t mmwDemo_registerBurstInterrupt(void);
#if 0
int32_t mmwDemo_registerChirpAvailableInterrupts(void);
#endif

int32_t mmwDemo_mmWaveInit(bool iswarmstrt);
// RPMF
// Priority of Power task
#define POWER_TASK_PRI  (2u)
// Stack for Power task
#define POWER_TASK_SIZE (1024u)
#define LOW_PWR_MODE_DISABLE (0)
#define LOW_PWR_MODE_ENABLE (1)
#define LOW_PWR_TEST_MODE (2)
StackType_t gPowerTaskStack[POWER_TASK_SIZE] __attribute__((aligned(32)));
// Power task objects
StaticTask_t gPowerTaskObj;
TaskHandle_t gPowerTask;
//Power task function
void powerManagementTask(void *args);
// Power task semaphore objects
StaticSemaphore_t gPowerSemObj;
SemaphoreHandle_t gPowerSem;
// Status of DFP Re-init
volatile int mmwReinitStatus = 0;
Power_SleepState demoLowPwrStateTaken = POWER_NONE;

volatile unsigned long long test;
//For Sensor Stop
uint32_t sensorStop = 0;
extern int8_t isSensorStarted;

// For freeing the channels after Sensor Stop
static void mmwDemo_freeDmaChannels(EDMA_Handle edmaHandle);

// LED config
uint32_t gpioBaseAddrLed, pinNumLed;

extern volatile unsigned long long demoStartTime;
volatile unsigned long long demoEndTime, slpTimeus,lpdsLatency;
extern TaskHandle_t gDpcTask;
extern TaskHandle_t gTlvTask;
extern TaskHandle_t gAdcFileTask;
extern TaskHandle_t gCliTask;
extern int32_t CLI_MMWStart(void);
// char bootRst[6][15] = {"PORZ", "Warm Reset", "Deep Sleep", "Soft Reset", "STC_WARM", "STC_PORZ"};
double demoTimeus,frmPrdus;


// Re-init Function Declarations
void PowerClock_init();
void Pinmux_init();
void QSPI_init();
void EDMA_init();
void HWA_init();
void Drivers_uartInit();
void TimerP_init();

int power_idleresumehook(uint_fast16_t eventType, uintptr_t eventArg, uintptr_t clientArg);

//The function reads the FRAME_REF_TIMER that runs free at 40MHz
uint32_t Cycleprofiler_getTimeStamp(void)
{
    uint32_t *frameRefTimer;
    frameRefTimer = (uint32_t *) 0x5B000020;
    return *frameRefTimer;
}

#define HWA_MAX_NUM_DMA_TRIG_CHANNELS 16

/**
 *  @b Description
 *  @n
 *      The function allocates HWA DMA source channel from the pool
 *
 *  @param[in]  pool Handle to pool object.
 *
 *  @retval
 *      channel Allocated HWA trigger source channel
 */
static uint8_t DPC_ObjDet_HwaDmaTrigSrcChanPoolAlloc(HwaDmaTrigChanPoolObj *pool)
{
    uint8_t channel = 0xFF;
    if(pool->dmaTrigSrcNextChan < HWA_MAX_NUM_DMA_TRIG_CHANNELS)
    {
        channel = pool->dmaTrigSrcNextChan;
        pool->dmaTrigSrcNextChan++;
    }
    return channel;
}

/**
 *  @b Description
 *  @n
 *      The function resets HWA DMA source channel pool
 *
 *  @param[in]  pool Handle to pool object.
 *
 *  @retval
 *      none
 */
void DPC_ObjDet_HwaDmaTrigSrcChanPoolReset(HwaDmaTrigChanPoolObj *pool)
{
    pool->dmaTrigSrcNextChan = 0;
}

/**
 *  @b Description
 *  @n
 *      Utility function for reseting memory pool.
 *
 *  @param[in]  pool Handle to pool object.
 *
 *  \ingroup DPC_OBJDET__INTERNAL_FUNCTION
 *
 *  @retval
 *      none.
 */
static void DPC_ObjDet_MemPoolReset(MemPoolObj *pool)
{
    pool->currAddr = (uintptr_t)pool->cfg.addr;
    pool->maxCurrAddr = pool->currAddr;
}


/**
 *  @b Description
 *  @n
 *      Utility function for setting memory pool to desired address in the pool.
 *      Helps to rewind for example.
 *
 *  @param[in]  pool Handle to pool object.
 *  @param[in]  addr Address to assign to the pool's current address.
 *
 *  \ingroup DPC_OBJDET__INTERNAL_FUNCTION
 *
 *  @retval
 *      None
 */
static void DPC_ObjDet_MemPoolSet(MemPoolObj *pool, void *addr)
{
    pool->currAddr = (uintptr_t)addr;
    pool->maxCurrAddr = MAX(pool->currAddr, pool->maxCurrAddr);
}

/**
 *  @b Description
 *  @n
 *      Utility function for getting memory pool current address.
 *
 *  @param[in]  pool Handle to pool object.
 *
 *  \ingroup DPC_OBJDET__INTERNAL_FUNCTION
 *
 *  @retval
 *      pointer to current address of the pool (from which next allocation will
 *      allocate to the desired alignment).
 */
static void *DPC_ObjDet_MemPoolGet(MemPoolObj *pool)
{
    return((void *)pool->currAddr);
}

/**
 *  @b Description
 *  @n
 *      Utility function for getting maximum memory pool usage.
 *
 *  @param[in]  pool Handle to pool object.
 *
 *  \ingroup DPC_OBJDET__INTERNAL_FUNCTION
 *
 *  @retval
 *      Amount of pool used in bytes.
 */
static uint32_t DPC_ObjDet_MemPoolGetMaxUsage(MemPoolObj *pool)
{
    return((uint32_t)(pool->maxCurrAddr - (uintptr_t)pool->cfg.addr));
}

/**
 *  @b Description
 *  @n
 *      Utility function for allocating from a static memory pool.
 *
 *  @param[in]  pool Handle to pool object.
 *  @param[in]  size Size in bytes to be allocated.
 *  @param[in]  align Alignment in bytes
 *
 *  \ingroup DPC_OBJDET__INTERNAL_FUNCTION
 *
 *  @retval
 *      pointer to beginning of allocated block. NULL indicates could not
 *      allocate.
 */
static void *DPC_ObjDet_MemPoolAlloc(MemPoolObj *pool,
                              uint32_t size,
                              uint8_t align)
{
    void *retAddr = NULL;
    uintptr_t addr;

    addr = MEM_ALIGN(pool->currAddr, align);
    if ((addr + size) <= ((uintptr_t)pool->cfg.addr + pool->cfg.size))
    {
        retAddr = (void *)addr;
        pool->currAddr = addr + size;
        pool->maxCurrAddr = MAX(pool->currAddr, pool->maxCurrAddr);
    }

    return(retAddr);
}
void *classifier_malloc(uint32_t sizeInBytes)
{
    return HeapP_alloc(&gMmwMssMCB.CoreLocalFeatExtractHeapObj, sizeInBytes);
}
void classifier_free(void *pFree, uint32_t sizeInBytes)
{
    HeapP_free(&gMmwMssMCB.CoreLocalFeatExtractHeapObj, pFree);
}


void featExtract_heapConstruct()
{
    HeapP_construct(&gMmwMssMCB.CoreLocalFeatExtractHeapObj, (void *) gMmwCoreLocMem3, MMWDEMO_OBJDET_CORE_LOCAL_MEM3_SIZE);
}

void *featExtract_malloc(uint32_t sizeInBytes)
{
    return HeapP_alloc(&gMmwMssMCB.CoreLocalFeatExtractHeapObj, sizeInBytes);
}
void featExtract_free(void *pFree, uint32_t sizeInBytes)
{
    HeapP_free(&gMmwMssMCB.CoreLocalFeatExtractHeapObj, pFree);
}
uint32_t featExtract_memUsage()
{
    uint32_t usedMemSizeInBytes;
    HeapP_MemStats heapStats;

    HeapP_getHeapStats(&gMmwMssMCB.CoreLocalFeatExtractHeapObj, &heapStats);
    usedMemSizeInBytes = sizeof(gMmwCoreLocMem3) - heapStats.availableHeapSpaceInBytes;

    return usedMemSizeInBytes;
}




/**
 *  @b Description
 *  @n
 *      This function reads calibration data from flash and send it to front end
 *
 *  @param[in]  ptrCalibData         Pointer to Calibration data
 *
 *  @retval
 *      Success -   0
 *  @retval
 *      Error   -   <0
 */
static int32_t MmwDemo_calibRestore(MmwDemo_calibData  *ptrCalibData)
{
    int32_t    retVal = 0;
    uint32_t   flashOffset;

    /* Get Flash Offset */
    flashOffset = gMmwMssMCB.factoryCalCfg.flashOffset;

    /* Read calibration data */
    if(mmwDemo_flashRead(flashOffset, (uint8_t *)ptrCalibData, sizeof(MmwDemo_calibData) )< 0)
    {
        /* Error: only one can be enable at at time */
        CLI_write ("Error: MmwDemo failed when reading calibration data from flash.\r\n");
        return -1;
    }

    /* Validate Calib data Magic number */
    if(ptrCalibData->magic != MMWDEMO_CALIB_STORE_MAGIC)
    {
        /* Header validation failed */
        CLI_write ("Error: MmwDemo calibration data header validation failed.\r\n");
        return -1;
    }

    return retVal;
}

/**
 *  @b Description
 *  @n
 *      This function retrieves the calibration data from front end and saves it in flash.
 *
 *  @param[in]  ptrCalibrationData      Pointer to Calibration data
 *
 *  @retval
 *      Success -   0
 *  @retval
 *      Error   -   <0
 */
static int32_t MmwDemo_calibSave(MmwDemo_calibData  *ptrCalibrationData)
{
    uint32_t                flashOffset;
    int32_t                 retVal = 0;

    /* Calculate the read size in bytes */
    flashOffset = gMmwMssMCB.factoryCalCfg.flashOffset;

    /* Flash calibration data */
    retVal = mmwDemo_flashWrite(flashOffset, (uint8_t *)ptrCalibrationData, sizeof(MmwDemo_calibData));
    if(retVal < 0)
    {
        /* Flash write failed */
        CLI_write ("Error: MmwDemo failed flashing calibration data with error[%d].\n", retVal);
    }

    return retVal;
}


/**
 *  @b Description
 *  @n
 *      Send assert information through CLI.
 */
void _MmwDemo_debugAssert(int32_t expression, const char *file, int32_t line)
{
    if (!expression) {
        CLI_write ("Exception: %s, line %d.\r\n",file,line);
    }
}


#if 0
void mmwDemoBurstISR(void *arg)
{
    HwiP_clearInt(CSL_APPSS_INTR_MUXED_FECSS_CHIRPTIMER_BURST_START_AND_BURST_END);
    gMmwDemoBurstCnt++;
}

void mmwDemoChirpStartISR(void *arg)
{
    HwiP_clearInt(CSL_APPSS_INTR_MUXED_FECSS_CHIRPTIMER_CHIRP_START_AND_CHIRP_END);
    gMmwDemoChirpStartCnt++;
}


static void mmwDemoChirpISR(void *arg)
{
    HwiP_clearInt(CSL_APPSS_INTR_MUXED_FECSS_CHIRP_AVAIL_IRQ_AND_ADC_VALID_START_AND_SYNC_IN); //CSL_MSS_INTR_RSS_ADC_CAPTURE_COMPLETE
    gMmwDemoChirpCnt++;
}
#endif

/* In order to debug target code (set brake points, step over,...) set this variable below to 1 in CCS
 * expression window. It will prevent ISR mmwDemoFrameStartISR from forcing the code to stop */
volatile uint32_t gDebugTargetCode = 1;

/**
*  @b Description
*  @n
*    Frame start ISR
*/
static void mmwDemoFrameStartISR(void *arg)
{
    uint32_t curCycle;
    MmwDemo_MSS_MCB *mmwMssMCB = (MmwDemo_MSS_MCB *) arg;

    HwiP_clearInt(CSL_APPSS_INTR_FECSS_FRAMETIMER_FRAME_START);

    if (gMmwMssMCB.lowPowerMode == LOW_PWR_MODE_DISABLE)
    {
        demoStartTime = PRCMSlowClkCtrGet();
    }
    mmwMssMCB->frameStartTimeStampSlowClk = PRCMSlowClkCtrGet();
    curCycle = Cycleprofiler_getTimeStamp();
    /* For testing */
    mmwMssMCB->stats.framePeriod_us = (curCycle - mmwMssMCB->stats.frameStartTimeStamp)/FRAME_REF_TIMER_CLOCK_MHZ;
    mmwMssMCB->stats.frameStartTimeStamp = curCycle;

    if (gDebugTargetCode == 0)
    {
        DebugP_assert(mmwMssMCB->interSubFrameProcToken == 0);
    }

    if(mmwMssMCB->interSubFrameProcToken > 0)
    {
        mmwMssMCB->interSubFrameProcOverflowCntr++;
    }

    mmwMssMCB->interSubFrameProcToken++;

    mmwMssMCB->stats.frameStartIntCounter++;
}

static int32_t MmwDemo_calibInit(void)
{
    int32_t        retVal = 0;
    ATE_CalibData  *ateCalib = (ATE_CalibData *)&gATECalibDataStorage;

    gMmwMssMCB.mmwAteCalibCfg.flashOffset = ATE_CALIB_FLASH_OFFSET;
    gMmwMssMCB.mmwAteCalibCfg.restoreEnable = 1;
    gMmwMssMCB.mmwAteCalibCfg.sizeOfCalibDataStorage = ATE_CALIB_DATA_LENGTH;

    /* Check if Calibration data is over the Reserved storage */
    if(gMmwMssMCB.mmwAteCalibCfg.restoreEnable == 1)
    {
        /* Resets calibration data */
        memset((void *)&gATECalibDataStorage, 0, sizeof(gATECalibDataStorage));

        /* Initialize Flash interface. */
        retVal = mmwDemo_flashInit();

        /* Check if the device is RF-Trimmed */
        /* Checking one Trim is enough*/
        if(SOC_rcmReadSynthTrimValid() == RF_SYNTH_TRIM_VALID)
        {
            /* Set this flag to valid as ATE calibration is read from Efuse */
            ateCalib->validityFlag = ATE_CALIB_DATA_VALID;
            gMmwMssMCB.factoryCalCfg.atecalibinEfuse = true;
        }
        else
        {
            gMmwMssMCB.factoryCalCfg.atecalibinEfuse = false;
            /* Read ATE Calibration data from Flash memory offset. */
            retVal = mmwDemo_flashRead(gMmwMssMCB.mmwAteCalibCfg.flashOffset,
                                   (uint8_t*)&gATECalibDataStorage, sizeof(gATECalibDataStorage));

            /* Check the validity of the Calibration data before populating address in mmwave Control MCB */
            if(ateCalib->validityFlag == ATE_CALIB_DATA_VALID )
            {
                DebugP_log("Calibration Validated for restore \n");
            }
            else
            {
                DebugP_log("Calibration Invalid \n");
            }
       }
    }
    else
    {
        CLI_write ("Error: Calibration restore not requested\n");
        retVal = -1;
    }

    return retVal;
}

/**
 *  @b Description
 *  @n
 *      Utility function to do a parabolic/quadratic fit on 3 input points
 *      and return the coordinates of the peak. This is used to accurately estimate
 *      range bias.
 *
 *  @param[in]  x Pointer to array of 3 elements representing the x-coordinate
 *              of the points to fit
 *  @param[in]  y Pointer to array of 3 elements representing the y-coordinate
 *              of the points to fit
 *  @param[out] xv Pointer to output x-coordinate of the peak value
 *  @param[out] yv Pointer to output y-coordinate of the peak value
 *
 *  @retval   None
 *
 */
static void DPC_ObjDet_quadFit(float *x, float*y, float *xv, float *yv)
{
    float a, b, c, denom;
    float x0 = x[0];
    float x1 = x[1];
    float x2 = x[2];
    float y0 = y[0];
    float y1 = y[1];
    float y2 = y[2];

    denom = (x0 - x1)*(x0 - x2)*(x1 - x2);
    if (denom != 0.)
    {
        a = (x2 * (y1 - y0) + x1 * (y0 - y2) + x0 * (y2 - y1)) / denom;
        b = (x2*x2 * (y0 - y1) + x1*x1 * (y2 - y0) + x0*x0 * (y1 - y2)) / denom;
        c = (x1 * x2 * (x1 - x2) * y0 + x2 * x0 * (x2 - x0) * y1 + x0 * x1 * (x0 - x1) * y2) / denom;
    }
    else
    {
        *xv = x[1];
        *yv = y[1];
        return;
    }
    if (a != 0.)
    {
        *xv = -b/(2*a);
        *yv = c - b*b/(4*a);
    }
    else
    {
        *xv = x[1];
        *yv = y[1];
    }
}


/**
 *  @b Description
 *  @n
 *      The function initializes parameters for the measurement procedure for rx channel compensation
 *
 *
 *  @retval   None
 *
 */
int32_t  mmwDemo_rangeBiasRxChPhaseMeasureConfig ()
{
    int32_t retVal = 0;
    int32_t i;
    float slope;
    float targetDistance = gMmwMssMCB.measureRxChannelBiasCliCfg.targetDistance;
    float searchWinSize= gMmwMssMCB.measureRxChannelBiasCliCfg.searchWinSize;


    //Check for CLI configuration, major mode, clutter removal disabled, TDM MIMO mode, all antennas active: 2Tx 3Rx, ...
    if ((!gMmwMssMCB.enableMajorMotion) ||
        (gMmwMssMCB.staticClutterRemovalEnable) ||
        (gMmwMssMCB.isBpmEnabled) ||
        (gMmwMssMCB.numTxAntennas != SYS_COMMON_NUM_TX_ANTENNAS) ||
        (gMmwMssMCB.numRxAntennas != SYS_COMMON_NUM_RX_CHANNEL))
    {
        retVal = -1;
    }

    if (retVal < 0)
    {
        goto exit;
    }

    /* Range step (meters/bin)*/
    slope = (float)(gMmwMssMCB.chirpSlope * 1.e12);
    gMmwMssMCB.measureRxChannelBiasParams.rangeStep = (MMWDEMO_RFPARSER_SPEED_OF_LIGHT_IN_METERS_PER_SEC * (gMmwMssMCB.adcSamplingRate * 1.e6)) /
                                                      (2.f * slope * (2*gMmwMssMCB.numRangeBins));
    gMmwMssMCB.measureRxChannelBiasParams.oneOverRangeStep = 1 / gMmwMssMCB.measureRxChannelBiasParams.rangeStep;


    /* Target position in bins */
    gMmwMssMCB.measureRxChannelBiasParams.trueBinPosition = targetDistance  * gMmwMssMCB.measureRxChannelBiasParams.oneOverRangeStep;

    /* Find the search range for the peak of the target at the bore sight */
    i = (int32_t) ((targetDistance - searchWinSize/2.) * gMmwMssMCB.measureRxChannelBiasParams.oneOverRangeStep + 0.5);
    if (i < 1)
    {
        i = 1;
    }
    gMmwMssMCB.measureRxChannelBiasParams.rngSearchLeftIdx = (int16_t) i;
    i = (int32_t) ((targetDistance + searchWinSize/2.) * gMmwMssMCB.measureRxChannelBiasParams.oneOverRangeStep + 0.5);
    gMmwMssMCB.measureRxChannelBiasParams.rngSearchRightIdx = (int16_t) i;

exit:
    return retVal;
}

/**
 *  @b Description
 *  @n
 *      Computes the range bias and rx phase compensation coefficients.
 *
 *  @retval   None
 *
 */
static void mmwDemo_rangeBiasRxChPhaseMeasure ()
{
    DPU_DoaProc_compRxChannelBiasFloatCfg *compRxChanCfg = &gMmwMssMCB.compRxChannelBiasCfgMeasureOut;
    cmplx16ImRe_t *symbolMatrix = (cmplx16ImRe_t *) gMmwMssMCB.radarCube[0].data; //Major motion

    cmplx16ImRe_t rxSym[SYS_COMMON_NUM_TX_ANTENNAS*SYS_COMMON_NUM_RX_CHANNEL];
    cmplx16ImRe_t *tempPtr;
    float sumSqr, sumSqrMax;
    float xMagSq[SYS_COMMON_NUM_TX_ANTENNAS*SYS_COMMON_NUM_RX_CHANNEL];
    int32_t iMax;
    float xMagSqMin, xMagSqRootMin;
    float scal;
    float truePosition = gMmwMssMCB.measureRxChannelBiasParams.trueBinPosition;
    float rangeStep = gMmwMssMCB.measureRxChannelBiasParams.rangeStep;
    float y[3];
    float x[3];
    float estPeakPos;
    float estPeakVal;
    int32_t i, ind;
    int32_t txIdx, rxIdx;

    uint32_t numRxAntennas = gMmwMssMCB.numRxAntennas;
    uint32_t numTxAntennas = gMmwMssMCB.numTxAntennas;
    uint32_t numVirtualAntennas = numRxAntennas * numTxAntennas;
    uint32_t numRangeBins = gMmwMssMCB.numRangeBins;

    uint32_t numSymPerTxAnt = numRxAntennas * numRangeBins;
    uint32_t symbolMatrixIndx;



    /**** Range calibration ****/
    iMax = gMmwMssMCB.measureRxChannelBiasParams.rngSearchLeftIdx;
    sumSqrMax = 0;
    for (i = gMmwMssMCB.measureRxChannelBiasParams.rngSearchLeftIdx; i <= gMmwMssMCB.measureRxChannelBiasParams.rngSearchRightIdx; i++)
    {
        sumSqr = 0.0;
        for (txIdx=0; txIdx < numTxAntennas; txIdx++)
        {
            for (rxIdx=0; rxIdx < numRxAntennas; rxIdx++)
            {
                symbolMatrixIndx = txIdx * numSymPerTxAnt + rxIdx * numRangeBins + i;
                tempPtr = (cmplx16ImRe_t *) &symbolMatrix[symbolMatrixIndx];
                sumSqr += (float) tempPtr->real * (float) tempPtr->real +
                          (float) tempPtr->imag * (float) tempPtr->imag;
            }
        }

        if (sumSqr > sumSqrMax)
        {
            sumSqrMax = sumSqr;
            iMax = i;
        }
    }

    /* Fine estimate of the peak position using quadratic fit */
    ind = 0;
    for (i = iMax-1; i <= iMax+1; i++)
    {
        sumSqr = 0.0;
        for (txIdx=0; txIdx < numTxAntennas; txIdx++)
        {
            for (rxIdx=0; rxIdx < numRxAntennas; rxIdx++)
            {
                symbolMatrixIndx = txIdx * numSymPerTxAnt + rxIdx * numRangeBins + i;
                tempPtr = (cmplx16ImRe_t *) &symbolMatrix[symbolMatrixIndx];
                sumSqr += (float) tempPtr->real * (float) tempPtr->real +
                          (float) tempPtr->imag * (float) tempPtr->imag;
            }
        }
        y[ind] = sqrtf(sumSqr);
        x[ind] = (float)i;
        ind++;
    }
    DPC_ObjDet_quadFit(x, y, &estPeakPos, &estPeakVal);
    compRxChanCfg->rangeBias = (estPeakPos - truePosition) * rangeStep;

    /*** Calculate Rx channel phase/gain compensation coefficients ***/
    for (txIdx = 0; txIdx < numTxAntennas; txIdx++)
    {
        for (rxIdx = 0; rxIdx < numRxAntennas; rxIdx++)
        {
            i = txIdx * numRxAntennas + rxIdx;
            symbolMatrixIndx = txIdx * numSymPerTxAnt + rxIdx * numRangeBins + iMax;
            rxSym[i] = symbolMatrix[symbolMatrixIndx];
            xMagSq[i] = (float) rxSym[i].real * (float) rxSym[i].real +
                        (float) rxSym[i].imag * (float) rxSym[i].imag;
        }
    }
    xMagSqMin = xMagSq[0];
    for (i = 1; i < numVirtualAntennas; i++)
    {
        if (xMagSq[i] < xMagSqMin)
        {
            xMagSqMin = xMagSq[i];
        }
    }

    if (xMagSqMin > 0.)
    {
	    xMagSqRootMin = sqrt(xMagSqMin);
	    for (txIdx=0; txIdx < numTxAntennas; txIdx++)
	    {
	        for (rxIdx=0; rxIdx < numRxAntennas; rxIdx++)
	        {
	            float temp;
	            i = txIdx * numRxAntennas + rxIdx;
	            scal = 1./ xMagSq[i] * xMagSqRootMin;

	            temp = scal * rxSym[i].real;
	            compRxChanCfg->rxChPhaseComp[2*i] = temp;

	            temp = -scal * rxSym[i].imag;
	            compRxChanCfg->rxChPhaseComp[2*i+1] = temp;
	        }
	    }
    }
    else
    {
	    for (i=0; i < (2*numVirtualAntennas); i++)
	    {
	            compRxChanCfg->rxChPhaseComp[i] = 0.;
	    }
    }
}

/**
*  @b Description
*  @n
*    Select coordinates of active virtual antennas and calculate the size of the 2D virtual antenna pattern,
*    i.e. number of antenna rows and number of antenna columns.
*/
void MmwDemo_calcActiveAntennaGeometry()
{
    int32_t txInd, rxInd, ind;
    int32_t rowMax, colMax;
    int32_t rowMin, colMin;
    /* Select only active antennas */
    ind = 0;
    for (txInd = 0; txInd < gMmwMssMCB.numTxAntennas; txInd++)
    {
        for (rxInd = 0; rxInd < gMmwMssMCB.numRxAntennas; rxInd++)
        {
            gMmwMssMCB.activeAntennaGeometryCfg.ant[ind] = gMmwMssMCB.antennaGeometryCfg.ant[gMmwMssMCB.rxAntOrder[rxInd] + (txInd * SYS_COMMON_NUM_RX_CHANNEL)];
            ind++;
        }
    }

    /* Calculate virtual antenna 2D array size */
    ind = 0;
    rowMax = 0;
    colMax = 0;
    rowMin = 127;
    colMin = 127;
    for (txInd = 0; txInd < gMmwMssMCB.numTxAntennas; txInd++)
    {
        for (rxInd = 0; rxInd < gMmwMssMCB.numRxAntennas; rxInd++)
        {
            if (gMmwMssMCB.activeAntennaGeometryCfg.ant[ind].row > rowMax)
            {
                rowMax = gMmwMssMCB.activeAntennaGeometryCfg.ant[ind].row;
            }
            if (gMmwMssMCB.activeAntennaGeometryCfg.ant[ind].col > colMax)
            {
                colMax = gMmwMssMCB.activeAntennaGeometryCfg.ant[ind].col;
            }
            if (gMmwMssMCB.activeAntennaGeometryCfg.ant[ind].row < rowMin)
            {
                rowMin = gMmwMssMCB.activeAntennaGeometryCfg.ant[ind].row;
            }
            if (gMmwMssMCB.activeAntennaGeometryCfg.ant[ind].col < colMin)
            {
                colMin = gMmwMssMCB.activeAntennaGeometryCfg.ant[ind].col;
            }
            ind++;
        }
    }
    ind = 0;
    for (txInd = 0; txInd < gMmwMssMCB.numTxAntennas; txInd++)
    {
        for (rxInd = 0; rxInd < gMmwMssMCB.numRxAntennas; rxInd++)
        {
            gMmwMssMCB.activeAntennaGeometryCfg.ant[ind].row -= rowMin;
            gMmwMssMCB.activeAntennaGeometryCfg.ant[ind].col -= colMin;
            ind++;
        }
    }
    gMmwMssMCB.numAntRow = rowMax - rowMin + 1;
    gMmwMssMCB.numAntCol = colMax - colMin + 1;
}

#define DOPPLER_OUTPUT_MAPPING_DOP_ROW_COL   0
#define DOPPLER_OUTPUT_MAPPING_ROW_DOP_COL   1


/**
*  @b Description
*  @n
*    Based on the activeAntennaGeometryCfg configures the table which used to configure
*    Doppler FFT HWA param sets in DoA DPU. THese param sets perform Doppler FFT and
*    at the same time mapping of input antennas into 2D row-column antenna array where columns
*    are in  azimuth dimension, and rows in elevation dimension.
*    It also calculates the size of 2D antenna array, ie. number of rows and number of columns.
*/
int32_t MmwDemo_cfgDopplerParamMapping(DPU_DoaProc_HWA_Option_Cfg *dopplerParamCfg, uint32_t mappingOption)
{
    int32_t ind, indNext, indNextPrev;
    int32_t row, col;
    int32_t dopParamInd;
    int32_t state;
    int16_t BT[DPU_DOA_PROC_MAX_2D_ANT_ARRAY_ELEMENTS];
    int16_t DT[DPU_DOA_PROC_MAX_2D_ANT_ARRAY_ELEMENTS];
    int16_t SCAL[DPU_DOA_PROC_MAX_2D_ANT_ARRAY_ELEMENTS];
    int8_t  DONE[DPU_DOA_PROC_MAX_2D_ANT_ARRAY_ELEMENTS];
    int32_t retVal = 0;
    int32_t rowOffset;

    if (gMmwMssMCB.numAntRow * gMmwMssMCB.numAntCol > DPU_DOA_PROC_MAX_2D_ANT_ARRAY_ELEMENTS)
    {
        retVal = DPC_OBJECTDETECTION_EANTENNA_GEOMETRY_CFG_FAILED;
        goto exit;
    }

    if (mappingOption == DOPPLER_OUTPUT_MAPPING_DOP_ROW_COL)
    {
        /*For AOA DPU, Output is */
        rowOffset =  gMmwMssMCB.numAntCol;
    }
    else if (mappingOption == DOPPLER_OUTPUT_MAPPING_ROW_DOP_COL)
    {
        rowOffset =  gMmwMssMCB.numDopplerBins * gMmwMssMCB.numAntCol;
    }
    else
    {
        retVal = DPC_OBJECTDETECTION_EANTENNA_GEOMETRY_CFG_FAILED;
        goto exit;
    }

    /* Initialize tables */
    for (ind = 0; ind < (gMmwMssMCB.numAntRow * gMmwMssMCB.numAntCol); ind++)
    {
        BT[ind] = 0;
        SCAL[ind] = 0;
        DONE[ind] = 0;
    }

    for (ind = 0; ind < (gMmwMssMCB.numTxAntennas * gMmwMssMCB.numRxAntennas); ind++)
    {
        row = gMmwMssMCB.activeAntennaGeometryCfg.ant[ind].row;
        col = gMmwMssMCB.activeAntennaGeometryCfg.ant[ind].col;
        BT[row * gMmwMssMCB.numAntCol + col] = ind;
        SCAL[row * gMmwMssMCB.numAntCol + col] = 1;
    }
    for (row = 0; row < gMmwMssMCB.numAntRow; row++)
    {
        for (col = 0; col < gMmwMssMCB.numAntCol; col++)
        {
            ind = row * gMmwMssMCB.numAntCol + col;
            DT[ind] = row * rowOffset + col;
        }
    }


    /* Configure Doppler HWA mapping params for antenna mapping */
    dopParamInd = 0;
    dopplerParamCfg->numDopFftParams = 0;
    for (ind = 0; ind < (gMmwMssMCB.numAntRow * gMmwMssMCB.numAntCol); ind++)
    {
        if (!DONE[ind])
        {
            if(dopParamInd == DPU_DOA_PROC_MAX_NUM_DOP_FFFT_PARAMS)
            {
                retVal = DPC_OBJECTDETECTION_EANTENNA_GEOMETRY_CFG_FAILED;
                goto exit;
            }

            DONE[ind] = 1;
            dopplerParamCfg->numDopFftParams++;
            dopplerParamCfg->dopFftCfg[dopParamInd].srcBcnt = 1;
            dopplerParamCfg->dopFftCfg[dopParamInd].scale = SCAL[ind];
            if (dopplerParamCfg->dopFftCfg[dopParamInd].scale == 0)
            {
                dopplerParamCfg->dopFftCfg[dopParamInd].srcAddrOffset = 0;
            }
            else
            {
                dopplerParamCfg->dopFftCfg[dopParamInd].srcAddrOffset = BT[ind];
            }
            dopplerParamCfg->dopFftCfg[dopParamInd].dstAddrOffset = DT[ind];
            state = 1;//STATE_SECOND:
            for (indNext = ind+1; indNext < (gMmwMssMCB.numAntRow * gMmwMssMCB.numAntCol); indNext++)
            {

                if (!DONE[indNext] && (dopplerParamCfg->dopFftCfg[dopParamInd].scale == SCAL[indNext]))
                {
                    switch (state)
                    {
                        case 1://STATE_SECOND:
                            dopplerParamCfg->dopFftCfg[dopParamInd].srcBcnt++;
                            DONE[indNext] = 1;
                            if (SCAL[indNext] == 1)
                            {
                                dopplerParamCfg->dopFftCfg[dopParamInd].srcBidx = BT[indNext] - dopplerParamCfg->dopFftCfg[dopParamInd].srcAddrOffset;
                            }
                            else
                            {
                                dopplerParamCfg->dopFftCfg[dopParamInd].srcBidx = 0;
                            }
                            dopplerParamCfg->dopFftCfg[dopParamInd].dstBidx = DT[indNext] - DT[ind];
                            indNextPrev = indNext;
                            state = 2;//STATE_NEXT:
                            break;
                        case 2://STATE_NEXT:
                            if (SCAL[indNext] == 1)
                            {
                                if ((dopplerParamCfg->dopFftCfg[dopParamInd].srcBidx == (BT[indNext] - BT[indNextPrev])) &&
                                    (dopplerParamCfg->dopFftCfg[dopParamInd].dstBidx == (DT[indNext] - DT[indNextPrev])))
                                {
                                    DONE[indNext] = 1;
                                    dopplerParamCfg->dopFftCfg[dopParamInd].srcBcnt++;
                                    indNextPrev = indNext;
                                }
                            }
                            else
                            {
                                if (dopplerParamCfg->dopFftCfg[dopParamInd].dstBidx == (DT[indNext] - DT[indNextPrev]))
                                {
                                    DONE[indNext] = 1;
                                    dopplerParamCfg->dopFftCfg[dopParamInd].srcBcnt++;
                                    indNextPrev = indNext;
                                }
                            }
                            break;
                    }
                }
            }
            dopParamInd++;
        }
    }

    dopplerParamCfg->numDopFftParams = dopParamInd;

exit:
    return retVal;
}

/**
*  @b Description
*  @n
*    Based on the configuration, set up the range processing DPU configurations
*/
int32_t RangeProc_setProfile()
{

    int32_t retVal = 0;
    DPU_RangeProcHWA_HW_Resources *pHwConfig = &rangeProcDpuCfg.hwRes;
    DPU_RangeProcHWA_StaticConfig  * params;
    uint32_t index;
    uint32_t bytesPerRxChan;

    /* Rangeproc DPU */
    pHwConfig = &rangeProcDpuCfg.hwRes;
    params = &rangeProcDpuCfg.staticCfg;

    memset((void *)&rangeProcDpuCfg, 0, sizeof(DPU_RangeProcHWA_Config));

    params->enableMajorMotion = gMmwMssMCB.enableMajorMotion;
    params->enableMinorMotion = gMmwMssMCB.enableMinorMotion;

    params->numFramesPerMinorMotProc = gMmwMssMCB.sigProcChainCfg.numFrmPerMinorMotProc;
    params->numMinorMotionChirpsPerFrame = gMmwMssMCB.sigProcChainCfg.numMinorMotionChirpsPerFrame;
    params->frmCntrModNumFramesPerMinorMot = gMmwMssMCB.frmCntrModNumFramesPerMinorMot;
    params->lowPowerMode = gMmwMssMCB.lowPowerMode;

    gMmwMssMCB.frmCntrModNumFramesPerMinorMot++;
    if(gMmwMssMCB.frmCntrModNumFramesPerMinorMot == gMmwMssMCB.sigProcChainCfg.numFrmPerMinorMotProc)
    {
        gMmwMssMCB.frmCntrModNumFramesPerMinorMot = 0;
    }

    /* hwi configuration */
    pHwConfig = &rangeProcDpuCfg.hwRes;

    /* HWA configurations, not related to per test, common to all test */
    pHwConfig->hwaCfg.paramSetStartIdx = 0;
    pHwConfig->hwaCfg.numParamSet = DPU_RANGEPROCHWA_NUM_HWA_PARAM_SETS;
    pHwConfig->hwaCfg.hwaWinRamOffset  = DPC_OBJDET_HWA_WINDOW_RAM_OFFSET;
    pHwConfig->hwaCfg.hwaWinSym = 1;
    pHwConfig->hwaCfg.dataInputMode = DPU_RangeProcHWA_InputMode_ISOLATED;
    pHwConfig->hwaCfg.dmaTrigSrcChan[0] = DPC_ObjDet_HwaDmaTrigSrcChanPoolAlloc(&gMmwMssMCB.HwaDmaChanPoolObj);
    pHwConfig->hwaCfg.dmaTrigSrcChan[1] = DPC_ObjDet_HwaDmaTrigSrcChanPoolAlloc(&gMmwMssMCB.HwaDmaChanPoolObj);


    /* edma configuration */
    pHwConfig->edmaHandle  = gEdmaHandle[0];
    /* edma configuration depends on the interleave or non-interleave */

    /* windowing buffer is fixed, size will change*/
    params->windowSize = sizeof(uint32_t) * ((gMmwMssMCB.profileComCfg.h_NumOfAdcSamples +1 ) / 2); //symmetric window, for real samples
    params->window =  (int32_t *)DPC_ObjDet_MemPoolAlloc(&gMmwMssMCB.CoreLocalRamObj,
                                                         params->windowSize,
                                                         sizeof(uint32_t));
    if (params->window == NULL)
    {
        retVal = DPC_OBJECTDETECTION_ENOMEM__CORE_LOCAL_RAM_RANGE_HWA_WINDOW;
        goto exit;
    }

    /* adc buffer buffer, format fixed, interleave, size will change */
    params->ADCBufData.dataProperty.dataFmt = DPIF_DATAFORMAT_REAL16;
    params->ADCBufData.dataProperty.adcBits = 2U; // 12-bit only
    params->ADCBufData.dataProperty.numChirpsPerChirpEvent = 1U;

    if(gMmwMssMCB.adcDataSourceCfg.source == 0)
    {
        params->ADCBufData.data = (void *)CSL_APP_HWA_ADCBUF_RD_U_BASE;
    }
    else
    {
        gMmwMssMCB.adcTestBuff  = (uint8_t *) DPC_ObjDet_MemPoolAlloc(&gMmwMssMCB.L3RamObj,
                                                                            MMW_DEMO_TEST_ADC_BUFF_SIZE,
                                                                            sizeof(uint32_t));
        if(gMmwMssMCB.adcTestBuff == NULL)
        {
            retVal = DPC_OBJECTDETECTION_ENOMEM__L3_RAM_ADC_TEST_BUFF;
            goto exit;
        }
        params->ADCBufData.data = (void *)gMmwMssMCB.adcTestBuff;

    }

    params->numTxAntennas = (uint8_t) gMmwMssMCB.numTxAntennas;
    params->numVirtualAntennas = (uint8_t) (gMmwMssMCB.numTxAntennas * gMmwMssMCB.numRxAntennas);
    params->numRangeBins = gMmwMssMCB.numRangeBins;
    params->numChirpsPerFrame = gMmwMssMCB.frameCfg.h_NumOfBurstsInFrame * gMmwMssMCB.frameCfg.h_NumOfChirpsInBurst;
    params->numDopplerChirpsPerFrame = params->numChirpsPerFrame/params->numTxAntennas;

    if ((params->numTxAntennas == 1) && (gMmwMssMCB.frameCfg.h_NumOfBurstsInFrame !=1))
    {
        retVal = DPC_OBJECTDETECTION_EINVAL_CFG;
        goto exit;
    }
    if ((params->numTxAntennas == 1) && (gMmwMssMCB.isBpmEnabled))
    {
        retVal = DPC_OBJECTDETECTION_EINVAL_CFG;
        goto exit;
    }

    if (params->enableMajorMotion)
    {
        params->numDopplerChirpsPerProc = params->numDopplerChirpsPerFrame;
    }
    else
    {
        params->numDopplerChirpsPerProc = params->numFramesPerMinorMotProc * params->numMinorMotionChirpsPerFrame;
    }

    params->isBpmEnabled = gMmwMssMCB.isBpmEnabled;
    /* windowing */
    params->ADCBufData.dataProperty.numRxAntennas = (uint8_t) gMmwMssMCB.numRxAntennas;
    params->ADCBufData.dataSize = gMmwMssMCB.profileComCfg.h_NumOfAdcSamples * params->ADCBufData.dataProperty.numRxAntennas * 4 ;
    params->ADCBufData.dataProperty.numAdcSamples = gMmwMssMCB.profileComCfg.h_NumOfAdcSamples;

    if (!gMmwMssMCB.oneTimeConfigDone)
    {
        mathUtils_genWindow((uint32_t *)params->window,
                            (uint32_t) params->ADCBufData.dataProperty.numAdcSamples,
                            params->windowSize/sizeof(uint32_t),
                            DPC_DPU_RANGEPROC_FFT_WINDOW_TYPE,
                            DPC_OBJDET_QFORMAT_RANGE_FFT);
    }
    params->rangeFFTtuning.fftOutputDivShift = 2;
    params->rangeFFTtuning.numLastButterflyStagesToScale = 0; /* no scaling needed as ADC is 16-bit and we have 8 bits to grow */

    params->rangeFftSize = mathUtils_pow2roundup(params->ADCBufData.dataProperty.numAdcSamples);

    bytesPerRxChan = params->ADCBufData.dataProperty.numAdcSamples * sizeof(uint16_t);
    bytesPerRxChan = (bytesPerRxChan + 15) / 16 * 16;

    for (index = 0; index < params->ADCBufData.dataProperty.numRxAntennas; index++)
    {
        params->ADCBufData.dataProperty.rxChanOffset[index] = index * bytesPerRxChan;
    }

    params->ADCBufData.dataProperty.interleave = DPIF_RXCHAN_NON_INTERLEAVE_MODE;
    /* Data Input EDMA */
    pHwConfig->edmaInCfg.dataIn.channel         = DPC_OBJDET_DPU_RANGEPROC_EDMAIN_CH;
    pHwConfig->edmaInCfg.dataIn.channelShadow[0]   = DPC_OBJDET_DPU_RANGEPROC_EDMAIN_SHADOW_PING;
    pHwConfig->edmaInCfg.dataIn.channelShadow[1]   = DPC_OBJDET_DPU_RANGEPROC_EDMAIN_SHADOW_PONG;
    pHwConfig->edmaInCfg.dataIn.eventQueue      = DPC_OBJDET_DPU_RANGEPROC_EDMAIN_EVENT_QUE;
    pHwConfig->edmaInCfg.dataInSignature.channel         = DPC_OBJDET_DPU_RANGEPROC_EDMAIN_SIG_CH;
    pHwConfig->edmaInCfg.dataInSignature.channelShadow   = DPC_OBJDET_DPU_RANGEPROC_EDMAIN_SIG_SHADOW;
    pHwConfig->edmaInCfg.dataInSignature.eventQueue      = DPC_OBJDET_DPU_RANGEPROC_EDMAIN_SIG_EVENT_QUE;
    pHwConfig->intrObj = intrObj_rangeProc;

    /* Data Output EDMA */
    pHwConfig->edmaOutCfg.path[0].evtDecim.channel = DPC_OBJDET_DPU_RANGEPROC_EVT_DECIM_PING_CH;
    pHwConfig->edmaOutCfg.path[0].evtDecim.channelShadow[0] = DPC_OBJDET_DPU_RANGEPROC_EVT_DECIM_PING_SHADOW_0;
    pHwConfig->edmaOutCfg.path[0].evtDecim.channelShadow[1] = DPC_OBJDET_DPU_RANGEPROC_EVT_DECIM_PING_SHADOW_1;
    pHwConfig->edmaOutCfg.path[0].evtDecim.eventQueue = DPC_OBJDET_DPU_RANGEPROC_EVT_DECIM_PING_EVENT_QUE;

    pHwConfig->edmaOutCfg.path[1].evtDecim.channel = DPC_OBJDET_DPU_RANGEPROC_EVT_DECIM_PONG_CH;
    pHwConfig->edmaOutCfg.path[1].evtDecim.channelShadow[0] = DPC_OBJDET_DPU_RANGEPROC_EVT_DECIM_PONG_SHADOW_0;
    pHwConfig->edmaOutCfg.path[1].evtDecim.channelShadow[1] = DPC_OBJDET_DPU_RANGEPROC_EVT_DECIM_PONG_SHADOW_1;
    pHwConfig->edmaOutCfg.path[1].evtDecim.eventQueue = DPC_OBJDET_DPU_RANGEPROC_EVT_DECIM_PONG_EVENT_QUE;

    pHwConfig->edmaOutCfg.path[0].dataOutMinor.channel = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_MINOR_PING_CH;
    pHwConfig->edmaOutCfg.path[0].dataOutMinor.channelShadow = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_MINOR_PING_SHADOW;
    pHwConfig->edmaOutCfg.path[0].dataOutMinor.eventQueue = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_MINOR_PING_EVENT_QUE;

    pHwConfig->edmaOutCfg.path[1].dataOutMinor.channel = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_MINOR_PONG_CH;
    pHwConfig->edmaOutCfg.path[1].dataOutMinor.channelShadow = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_MINOR_PONG_SHADOW;
    pHwConfig->edmaOutCfg.path[1].dataOutMinor.eventQueue = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_MINOR_PONG_EVENT_QUE;

    pHwConfig->edmaOutCfg.path[0].dataOutMajor.channel = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_MAJOR_PING_CH;
    pHwConfig->edmaOutCfg.path[0].dataOutMajor.channelShadow = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_MAJOR_PING_SHADOW;
    pHwConfig->edmaOutCfg.path[0].dataOutMajor.eventQueue = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_MAJOR_PING_EVENT_QUE;

    pHwConfig->edmaOutCfg.path[1].dataOutMajor.channel = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_MAJOR_PONG_CH;
    pHwConfig->edmaOutCfg.path[1].dataOutMajor.channelShadow = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_MAJOR_PONG_SHADOW;
    pHwConfig->edmaOutCfg.path[1].dataOutMajor.eventQueue = DPC_OBJDET_DPU_RANGEPROC_EDMAOUT_MAJOR_PONG_EVENT_QUE;

    /* Radar cube Minor Motion*/
    if (params->enableMinorMotion)
    {
        gMmwMssMCB.radarCube[1].dataSize = params->numRangeBins * params->numVirtualAntennas * sizeof(cmplx16ReIm_t) * params->numDopplerChirpsPerProc;
        gMmwMssMCB.radarCube[1].data  = (cmplx16ImRe_t *) DPC_ObjDet_MemPoolAlloc(&gMmwMssMCB.L3RamObj,
                                                                                     gMmwMssMCB.radarCube[1].dataSize,
                                                                                     sizeof(uint32_t));
        if(gMmwMssMCB.radarCube[1].data == NULL)
        {
            retVal = DPC_OBJECTDETECTION_ENOMEM__L3_RAM_RADAR_CUBE;
            goto exit;
        }

    }
    else
    {
        gMmwMssMCB.radarCube[1].data  = NULL;
        gMmwMssMCB.radarCube[1].dataSize = 0;
    }
    gMmwMssMCB.radarCube[1].datafmt = DPIF_RADARCUBE_FORMAT_6;
    rangeProcDpuCfg.hwRes.radarCubeMinMot = gMmwMssMCB.radarCube[1];

    /* Radar cube Major Motion*/
    if (params->enableMajorMotion)
    {

        gMmwMssMCB.radarCube[0].dataSize = params->numRangeBins * params->numVirtualAntennas * sizeof(cmplx16ReIm_t) * params->numDopplerChirpsPerProc;
        gMmwMssMCB.radarCube[0].data  = (cmplx16ImRe_t *) DPC_ObjDet_MemPoolAlloc(&gMmwMssMCB.L3RamObj,
                                                                               gMmwMssMCB.radarCube[0].dataSize,
                                                                               sizeof(uint32_t));
        if(gMmwMssMCB.radarCube[0].data == NULL)
        {
            retVal = DPC_OBJECTDETECTION_ENOMEM__L3_RAM_RADAR_CUBE;
            goto exit;
        }


    }
    else
    {
        gMmwMssMCB.radarCube[0].data  = NULL;
        gMmwMssMCB.radarCube[0].dataSize = 0;
    }
    gMmwMssMCB.radarCube[0].datafmt = DPIF_RADARCUBE_FORMAT_6;
    rangeProcDpuCfg.hwRes.radarCube = gMmwMssMCB.radarCube[0];

exit:
    return retVal;
}

/**
*  @b Description
*  @n
*    Based on the configuration, set up the doa processing DPU configurations
*/
int32_t DoaProc_setProfile()
{
    DPIF_DetMatrix dopplerIndexMatrix;
    DPIF_DetMatrix elevationIndexMatrix;

    /* Doaproc DPU */
    DPU_DoaProc_EdmaCfg *edmaCfg;
    DPU_DoaProc_HwaCfg *hwaCfg;
    int32_t winGenLen, i;
    int32_t retVal = 0;
    DPU_DoaProc_StaticConfig  *doaStaticCfg;

    memset((void *)&doaProcDpuCfg, 0, sizeof(DPU_DoaProc_Config));

    hwRes = &doaProcDpuCfg.hwRes;
    doaStaticCfg = &doaProcDpuCfg.staticCfg;
    edmaCfg = &hwRes->edmaCfg;
    hwaCfg = &hwRes->hwaCfg;

    /* Select active antennas from available antennas and calculate number of antennas rows and columns */
    MmwDemo_calcActiveAntennaGeometry();

    doaStaticCfg->numAntRow = gMmwMssMCB.numAntRow;
    doaStaticCfg->numAntCol = gMmwMssMCB.numAntCol;


    /* Angle dimension */
    if ((gMmwMssMCB.numAntRow > 1) && (gMmwMssMCB.numAntCol > 1))
    {
        gMmwMssMCB.angleDimension = 2;
    }
    else if ((gMmwMssMCB.numAntRow == 1) && (gMmwMssMCB.numAntCol > 1))
    {
        gMmwMssMCB.angleDimension = 1;
    }
    else
    {
        gMmwMssMCB.angleDimension = 0;
    }


    doaStaticCfg->enableMajorMotion = gMmwMssMCB.enableMajorMotion;
    doaStaticCfg->enableMinorMotion = gMmwMssMCB.enableMinorMotion;

    doaStaticCfg->numTxAntennas = (uint8_t) gMmwMssMCB.numTxAntennas;
    doaStaticCfg->numRxAntennas = (uint8_t) gMmwMssMCB.numRxAntennas;
    doaStaticCfg->numVirtualAntennas = (uint8_t) (gMmwMssMCB.numTxAntennas * gMmwMssMCB.numRxAntennas);
    doaStaticCfg->numRangeBins = gMmwMssMCB.numRangeBins;


    doaStaticCfg->numMinorMotionChirpsPerFrame = gMmwMssMCB.sigProcChainCfg.numMinorMotionChirpsPerFrame;
    doaStaticCfg->numFrmPerMinorMotProc = gMmwMssMCB.sigProcChainCfg.numFrmPerMinorMotProc;
    if (doaStaticCfg->numTxAntennas > 1)
    {
        if (doaStaticCfg->enableMajorMotion)
        {
            if ((gMmwMssMCB.frameCfg.h_NumOfBurstsInFrame > 1) && (gMmwMssMCB.frameCfg.h_NumOfChirpsInBurst == 2))
            {
                /* Burst mode: h_NumOfBurstsInFrame > 1, h_NumOfChirpsInBurst = 2 */
                doaStaticCfg->numDopplerChirps   = gMmwMssMCB.frameCfg.h_NumOfBurstsInFrame;
            }
            else if (gMmwMssMCB.frameCfg.h_NumOfBurstsInFrame == 1)
            {
                /* Normal mode: h_NumOfBurstsInFrame = 1, h_NumOfChirpsInBurst >= doaStaticCfg->numTxAntennas */
                doaStaticCfg->numDopplerChirps   = gMmwMssMCB.frameCfg.h_NumOfChirpsInBurst / doaStaticCfg->numTxAntennas;
            }
            else
            {
                retVal = DPU_DOAPROC_EINVAL;
                goto exit;
            }
        }
        else
        {
            /* Minor motion */
            doaStaticCfg->numDopplerChirps   = gMmwMssMCB.sigProcChainCfg.numFrmPerMinorMotProc * gMmwMssMCB.sigProcChainCfg.numMinorMotionChirpsPerFrame;
        }
    }
    else
    {
        /* 1Tx antenna */
        if (gMmwMssMCB.frameCfg.h_NumOfBurstsInFrame != 1)
        {
            retVal = DPU_DOAPROC_EINVAL;
            goto exit;
        }
        doaStaticCfg->numDopplerChirps = gMmwMssMCB.frameCfg.h_NumOfChirpsInBurst;
    }


    doaStaticCfg->numDopplerBins     = mathUtils_pow2roundup(doaStaticCfg->numDopplerChirps);
    doaStaticCfg->log2NumDopplerBins = mathUtils_ceilLog2(doaStaticCfg->numDopplerBins);

    gMmwMssMCB.numDopplerBins = doaStaticCfg->numDopplerBins; //ToDo Cleanup. We save this for later, it will be used by CFAR.

    doaStaticCfg->selectCoherentPeakInDopplerDim = gMmwMssMCB.sigProcChainCfg.coherentDoppler;
    doaStaticCfg->angleDimension        = gMmwMssMCB.angleDimension;
    doaStaticCfg->isDetMatrixLogScale   = false;
    doaStaticCfg->azimuthFftSize        = gMmwMssMCB.sigProcChainCfg.azimuthFftSize;
    doaStaticCfg->elevationFftSize      = gMmwMssMCB.sigProcChainCfg.elevationFftSize;
    doaStaticCfg->isStaticClutterRemovalEnabled = gMmwMssMCB.staticClutterRemovalEnable;
    doaStaticCfg->isRxChGainPhaseCompensationEnabled   = 1;
    doaStaticCfg->doaRangeLoopType = 0;

    /* Configure Doppler fft param sets for mapping antennas into 1D/2D virtual antenna array */
    retVal = MmwDemo_cfgDopplerParamMapping(&hwaCfg->doaRngGateCfg, DOPPLER_OUTPUT_MAPPING_DOP_ROW_COL);
    if (retVal < 0)
    {
        goto exit;
    }

    /* L3 - Detection Matrix */
    for (i=0; i<2; i++)
    {
        if (((i==0) && doaStaticCfg->enableMajorMotion) || ((i==1) && (doaStaticCfg->enableMinorMotion)))
        {
            if(doaStaticCfg->isDetMatrixLogScale)
            {
                gMmwMssMCB.detMatrix[i].dataSize = doaStaticCfg->numRangeBins * doaStaticCfg->azimuthFftSize * sizeof(uint16_t);
            }
            else
            {
                gMmwMssMCB.detMatrix[i].dataSize = doaStaticCfg->numRangeBins * doaStaticCfg->azimuthFftSize * sizeof(uint32_t);
            }
            gMmwMssMCB.detMatrix[i].data = DPC_ObjDet_MemPoolAlloc(&gMmwMssMCB.L3RamObj,
                                                        gMmwMssMCB.detMatrix[i].dataSize,
                                                        sizeof(uint32_t));
            if (gMmwMssMCB.detMatrix[i].data == NULL)
            {
                retVal = DPC_OBJECTDETECTION_ENOMEM__L3_RAM_DET_MATRIX;
                goto exit;
            }
            gMmwMssMCB.detMatrix[i].datafmt = DPC_DPU_DPIF_DETMATRIX_FORMAT_2;


            gMmwMssMCB.rangeProfile[i] = DPC_ObjDet_MemPoolAlloc(&gMmwMssMCB.L3RamObj,
                                                                 doaStaticCfg->numRangeBins * sizeof(uint32_t),
                                                                 sizeof(uint32_t));
            if (gMmwMssMCB.rangeProfile[i] == NULL)
            {
                retVal = DPC_OBJECTDETECTION_ENOMEM__CORE_LOCAL_RAM_RANGE_PROFILE;
                goto exit;
            }
        }
        else
        {
            gMmwMssMCB.detMatrix[i].dataSize = 0;
            gMmwMssMCB.detMatrix[i].data = NULL;
            gMmwMssMCB.detMatrix[i].datafmt = 0;
            gMmwMssMCB.rangeProfile[i] = NULL;
        }
    }


    /* L3 - Doppler Index Matrix */
    if ((doaStaticCfg->selectCoherentPeakInDopplerDim == 1) ||
        (doaStaticCfg->selectCoherentPeakInDopplerDim == 2))
    {
        if (doaStaticCfg->angleDimension == 2)
        {
            /* 2D-case -  with elevation */
            dopplerIndexMatrix.dataSize = doaStaticCfg->numRangeBins * doaStaticCfg->azimuthFftSize * doaStaticCfg->elevationFftSize * sizeof(uint8_t);
        }
        else
        {
            /* 1D-case -  no elevation */
            dopplerIndexMatrix.dataSize = doaStaticCfg->numRangeBins * doaStaticCfg->azimuthFftSize * sizeof(uint8_t);
        }
        dopplerIndexMatrix.data = DPC_ObjDet_MemPoolAlloc(&gMmwMssMCB.L3RamObj,
                                                          dopplerIndexMatrix.dataSize,
                                                          sizeof(uint8_t));
        if (dopplerIndexMatrix.data == NULL)
        {
            retVal = DPC_OBJECTDETECTION_ENOMEM__L3_RAM_DET_MATRIX;
            goto exit;
        }
        dopplerIndexMatrix.datafmt = DPC_DPU_DPIF_DETMATRIX_FORMAT_2;
    }
    else
    {
        /* Non-coherent combining along Doppler dimension, Doppler output = 0 */
        dopplerIndexMatrix.dataSize = 0;
        dopplerIndexMatrix.data = NULL;
        dopplerIndexMatrix.datafmt = 0;
    }

    if (doaStaticCfg->angleDimension == 2)
    {
        /* L3 - Elevation Index Matrix */
        elevationIndexMatrix.dataSize = doaStaticCfg->numRangeBins * doaStaticCfg->azimuthFftSize * sizeof(uint8_t);
        elevationIndexMatrix.data = DPC_ObjDet_MemPoolAlloc(&gMmwMssMCB.L3RamObj,
                                                            elevationIndexMatrix.dataSize,
                                                            sizeof(uint8_t));
        if (elevationIndexMatrix.data == NULL)
        {
            retVal = DPC_OBJECTDETECTION_ENOMEM__L3_RAM_DET_MATRIX;
            goto exit;
        }
        elevationIndexMatrix.datafmt = DPC_DPU_DPIF_DETMATRIX_FORMAT_2;
    }
    else
    {
        elevationIndexMatrix.dataSize = 0;
        elevationIndexMatrix.data = NULL;
        elevationIndexMatrix.datafmt = 0;
    }

    /* hwRes - copy these structures */
    hwRes->radarCube = gMmwMssMCB.radarCube[0];   //ToDo remove from here it is passed through the process function, set to NULL
    hwRes->radarCubeMinMot = gMmwMssMCB.radarCube[1]; //ToDo remove from here it is passed through the process function, set to NULL

    hwRes->dopplerIndexMatrix = dopplerIndexMatrix;
    hwRes->elevationIndexMatrix = elevationIndexMatrix;

    gMmwMssMCB.dopplerIndexMatrix = dopplerIndexMatrix;    //ToDo Do this outside, this is shared between DOA and CFAR
    gMmwMssMCB.elevationIndexMatrix = elevationIndexMatrix;

    /* hwRes - edmaCfg */
    edmaCfg->edmaHandle = gEdmaHandle[0];

    /* edmaIn - ping - minor motion*/
    edmaCfg->edmaIn.chunk[0].channel =            DPC_OBJDET_DPU_DOAPROC_EDMAIN_PING_CH;
    edmaCfg->edmaIn.chunk[0].channelShadow =      DPC_OBJDET_DPU_DOAPROC_EDMAIN_PING_SHADOW;
    edmaCfg->edmaIn.chunk[0].eventQueue =         DPC_OBJDET_DPU_DOAPROC_EDMAIN_PING_EVENT_QUE;

    /* edmaIn - pong - minor motion*/
    edmaCfg->edmaIn.chunk[1].channel =            DPC_OBJDET_DPU_DOAPROC_EDMAIN_PONG_CH;
    edmaCfg->edmaIn.chunk[1].channelShadow =      DPC_OBJDET_DPU_DOAPROC_EDMAIN_PONG_SHADOW;
    edmaCfg->edmaIn.chunk[1].eventQueue =         DPC_OBJDET_DPU_DOAPROC_EDMAIN_PONG_EVENT_QUE;

    /* edmaHotSig */
    edmaCfg->edmaHotSig.channel =             DPC_OBJDET_DPU_DOAPROC_EDMA_HOT_SIG_CH;
    edmaCfg->edmaHotSig.channelShadow =       DPC_OBJDET_DPU_DOAPROC_EDMA_HOT_SIG_SHADOW;
    edmaCfg->edmaHotSig.eventQueue =          DPC_OBJDET_DPU_DOAPROC_EDMA_HOT_SIG_EVENT_QUE;




    /* edmaOut - Detection Matrix */
    edmaCfg->edmaDetMatOut.channel =           DPC_OBJDET_DPU_DOAPROC_EDMAOUT_DET_MATRIX_CH;
    edmaCfg->edmaDetMatOut.channelShadow =     DPC_OBJDET_DPU_DOAPROC_EDMAOUT_DET_MATRIX_SHADOW;
    edmaCfg->edmaDetMatOut.eventQueue =        DPC_OBJDET_DPU_DOAPROC_EDMAOUT_DET_MATRIX_EVENT_QUE;

    /* edmaOut - Elevation Index Matrix */
    edmaCfg->elevIndMatOut.channel =       DPC_OBJDET_DPU_DOAPROC_EDMAOUT_ELEVIND_MATRIX_CH;
    edmaCfg->elevIndMatOut.channelShadow = DPC_OBJDET_DPU_DOAPROC_EDMAOUT_ELEVIND_MATRIX_SHADOW;
    edmaCfg->elevIndMatOut.eventQueue =    DPC_OBJDET_DPU_DOAPROC_EDMAOUT_ELEVIND_MATRIX_EVENT_QUE;

    /* edmaOut - Doppler Index Matrix */
    edmaCfg->dopIndMatOut.channel =        DPC_OBJDET_DPU_DOAPROC_EDMAOUT_DOPIND_MATRIX_CH;
    edmaCfg->dopIndMatOut.channelShadow =  DPC_OBJDET_DPU_DOAPROC_EDMAOUT_DOPIND_MATRIX_SHADOW;
    edmaCfg->dopIndMatOut.eventQueue =     DPC_OBJDET_DPU_DOAPROC_EDMAOUT_DOPIND_MATRIX_EVENT_QUE;

    edmaCfg->edmaInterLoopOut.channel =       DPC_OBJDET_DPU_DOAPROC_INTER_LOOP_EDMAOUT_DET_MATRIX_CH;
    edmaCfg->edmaInterLoopOut.channelShadow = DPC_OBJDET_DPU_DOAPROC_INTER_LOOP_EDMAOUT_DET_MATRIX_SHADOW;
    edmaCfg->edmaInterLoopOut.eventQueue =    DPC_OBJDET_DPU_DOAPROC_INTER_LOOP_EDMAOUT_DET_MATRIX_EVENT_QUE;

    edmaCfg->edmaInterLoopIn.channel =       DPC_OBJDET_DPU_DOAPROC_INTER_LOOP_EDMAIN_CH;
    edmaCfg->edmaInterLoopIn.channelShadow = DPC_OBJDET_DPU_DOAPROC_INTER_LOOP_EDMAIN_SHADOW;
    edmaCfg->edmaInterLoopIn.eventQueue =    DPC_OBJDET_DPU_DOAPROC_INTER_LOOP_EDMAIN_EVENT_QUE;

    edmaCfg->edmaInterLoopHotSig.channel =       DPC_OBJDET_DPU_DOAPROC_INTER_LOOP_EDMA_HOT_SIG_CH;
    edmaCfg->edmaInterLoopHotSig.channelShadow = DPC_OBJDET_DPU_DOAPROC_INTER_LOOP_EDMA_HOT_SIG_SHADOW;
    edmaCfg->edmaInterLoopHotSig.eventQueue =    DPC_OBJDET_DPU_DOAPROC_INTER_LOOP_EDMA_HOT_SIG_EVENT_QUE;

    edmaCfg->edmaInterLoopChainBack.channel =       DPC_OBJDET_DPU_DOAPROC_INTER_LOOP_EDMA_CHAIN_BACK_CH;
    edmaCfg->edmaInterLoopChainBack.channelShadow = DPC_OBJDET_DPU_DOAPROC_INTER_LOOP_EDMA_CHAIN_BACK_SHADOW;
    edmaCfg->edmaInterLoopChainBack.eventQueue =    DPC_OBJDET_DPU_DOAPROC_INTER_LOOP_EDMA_CHAIN_BACK_EVENT_QUE;
    edmaCfg->intrObj = &intrObj_doaProc;

    /* hwaCfg */
    hwaCfg->hwaMemInpAddr = CSL_APP_HWA_DMA0_RAM_BANK3_BASE; //ToDo Use this in DOA DPU, remove flag isRxChGainPhaseCompensationEnabled
    hwaCfg->numParamSets = 0;  //The number depends on the configuration, will be populated by DPU_DoaProc_config()
    hwaCfg->paramSetStartIdx = DPU_RANGEPROCHWA_NUM_HWA_PARAM_SETS;

    hwaCfg->dmaTrigSrcChan = DPC_ObjDet_HwaDmaTrigSrcChanPoolAlloc(&gMmwMssMCB.HwaDmaChanPoolObj);

    /* hwaCfg - window */
    //Share FFT window between azimuth and elevation FFT, window = [+1 -1 +1 -1 ... ]
    winGenLen = (doaStaticCfg->azimuthFftSize > doaStaticCfg->elevationFftSize) ? doaStaticCfg->azimuthFftSize : doaStaticCfg->elevationFftSize;
    hwaCfg->windowSize = winGenLen * sizeof(int32_t);

    hwaCfg->window = (int32_t *)DPC_ObjDet_MemPoolAlloc(&gMmwMssMCB.CoreLocalRamObj,
                                                        hwaCfg->windowSize,
                                                        sizeof(uint32_t));
    if (hwaCfg->window == NULL)
    {
        retVal = DPC_OBJECTDETECTION_ENOMEM__CORE_LOCAL_RAM_DOA_HWA_WINDOW;
        goto exit;
    }
    /*Alternate 1,-1,...*/
    for (i=0; i<winGenLen; i++)
    {
        hwaCfg->window[i] = (1 - 2 * (i & 0x1)) * ((1<<17) - 1);
    }

    hwaCfg->winRamOffset = doaStaticCfg->numRangeBins;
    hwaCfg->winSym = HWA_FFT_WINDOW_NONSYMMETRIC;

    /* Rx compensation coefficients */
    {
        int32_t rxInd, txInd;
        int32_t ind = 0;
        doaStaticCfg->compRxChanCfg.rangeBias = gMmwMssMCB.compRxChannelBiasCfg.rangeBias;
        for (txInd = 0; txInd < doaStaticCfg->numTxAntennas; txInd++)
        {
            for (rxInd = 0; rxInd < doaStaticCfg->numRxAntennas; rxInd++)
            {
                doaStaticCfg->compRxChanCfg.rxChPhaseComp[ind++] = gMmwMssMCB.compRxChannelBiasCfg.rxChPhaseComp[gMmwMssMCB.rxAntOrder[rxInd] + (txInd * SYS_COMMON_NUM_RX_CHANNEL)]; //TODO: staticCfg->txAntOrder[txInd] * SYS_COMMON_NUM_RX_CHANNEL
            }
        }
    }

    hwRes->interLoopDataBuffer = NULL;

exit:
return retVal;
}

/**
*  @b Description
*  @n
*    Based on the configuration, set up the CFAR processing DPU configurations
*/
int32_t CfarProc_setProfile()
{
    int32_t retVal = 0;
    float adcStart, startFreq, slope, bandwidth, centerFreq;
    DPU_CFARProcHWA_HW_Resources *pHwConfig;
    DPU_CFARProcHWA_StaticConfig  *params;

    /* CFARproc DPU */
    pHwConfig = &cfarProcDpuCfg.res;
    params = &cfarProcDpuCfg.staticCfg;

    memset((void *)&cfarProcDpuCfg, 0, sizeof(DPU_CFARProcHWA_Config));

    /* HWA configurations, not related to per test, common to all test */
    pHwConfig->hwaCfg.paramSetStartIdx = DPU_DOAPROC_MAX_NUM_HWA_PARAMSET + DPU_RANGEPROCHWA_NUM_HWA_PARAM_SETS;
    pHwConfig->hwaCfg.numParamSet = DPU_CFARPROCHWA_MAX_NUM_HWA_PARAMSET;
    pHwConfig->hwaCfg.dmaTrigSrcChan = DPC_ObjDet_HwaDmaTrigSrcChanPoolAlloc(&gMmwMssMCB.HwaDmaChanPoolObj);

    /* edma configuration */
    pHwConfig->edmaHandle  = gEdmaHandle[0];

    /* Data Input EDMA */
    pHwConfig->edmaHwaIn.channel         = DPC_OBJDET_DPU_CFAR_PROC_EDMAIN_CH;
    pHwConfig->edmaHwaIn.channelShadow   = DPC_OBJDET_DPU_CFAR_PROC_EDMAIN_SHADOW;
    pHwConfig->edmaHwaIn.eventQueue      = DPC_OBJDET_DPU_CFAR_PROC_EDMAIN_EVENT_QUE;
    pHwConfig->edmaHwaInSignature.channel         = DPC_OBJDET_DPU_CFAR_PROC_EDMAIN_SIG_CH;
    pHwConfig->edmaHwaInSignature.channelShadow   = DPC_OBJDET_DPU_CFAR_PROC_EDMAIN_SIG_SHADOW;
    pHwConfig->edmaHwaInSignature.eventQueue      = DPC_OBJDET_DPU_CFAR_PROC_EDMAIN_SIG_EVENT_QUE;
    pHwConfig->intrObj = &intrObj_cfarProc;

    /* Data Output EDMA */
    pHwConfig->edmaHwaOut.channel         = DPC_OBJDET_DPU_CFAR_PROC_EDMAOUT_RNG_PROFILE_CH;
    pHwConfig->edmaHwaOut.channelShadow   = DPC_OBJDET_DPU_CFAR_PROC_EDMAOUT_RNG_PROFILE_SHADOW;
    pHwConfig->edmaHwaOut.eventQueue      = DPC_OBJDET_DPU_CFAR_PROC_EDMAOUT_RNG_PROFILE_EVENT_QUE;

    /* Give M0 and M1 memory banks for detection matrix scratch. */
    pHwConfig->hwaMemInp = (uint16_t *) CSL_APP_HWA_DMA0_RAM_BANK0_BASE;
    pHwConfig->hwaMemInpSize = (CSL_APP_HWA_BANK_SIZE * 2) / sizeof(uint16_t);

    /* M2 bank: for CFAR detection list */
    pHwConfig->hwaMemOutDetList = (DPU_CFARProcHWA_CfarDetOutput *) CSL_APP_HWA_DMA0_RAM_BANK2_BASE;
    pHwConfig->hwaMemOutDetListSize = CSL_APP_HWA_BANK_SIZE /
                                sizeof(DPU_CFARProcHWA_CfarDetOutput);

    /* M3 bank: for maximum azimuth values per range bin  (range profile) */
    pHwConfig->hwaMemOutRangeProfile = (DPU_CFARProcHWA_HwaMaxOutput *) CSL_APP_HWA_DMA0_RAM_BANK3_BASE;

    /* dynamic config */
    cfarProcDpuCfg.dynCfg.cfarCfg   = &gMmwMssMCB.cfarCfg;
    cfarProcDpuCfg.dynCfg.fovRange  = &gMmwMssMCB.rangeSelCfg;
    cfarProcDpuCfg.dynCfg.fovAoaCfg    = &gMmwMssMCB.fovCfg;

    /* DPU Static config */
    params->detectionHeatmapType = DPU_CFAR_RANGE_AZIMUTH_HEATMAP;
    params->numRangeBins = gMmwMssMCB.numRangeBins;
    params->numDopplerBins     = gMmwMssMCB.numDopplerBins;
    params->log2NumDopplerBins = mathUtils_ceilLog2(params->numDopplerBins);

    params->selectCoherentPeakInDopplerDim = gMmwMssMCB.sigProcChainCfg.coherentDoppler;
    params->angleDimension        = gMmwMssMCB.angleDimension;
    params->isDetMatrixLogScale   = false;
    params->azimuthFftSize        = gMmwMssMCB.sigProcChainCfg.azimuthFftSize;
    params->elevationFftSize      = gMmwMssMCB.sigProcChainCfg.elevationFftSize;
    params->isStaticClutterRemovalEnabled = gMmwMssMCB.staticClutterRemovalEnable;

    gMmwMssMCB.adcStartTime         = (gMmwMssMCB.profileTimeCfg.h_ChirpAdcStartTime >> 10) * (1/gMmwMssMCB.adcSamplingRate); //us
    adcStart                        =   (gMmwMssMCB.adcStartTime * 1.e-6);
    startFreq                       =   (float)(gMmwMssMCB.startFreq * 1.e9);
    slope                           =   (float)(gMmwMssMCB.chirpSlope * 1.e12);
    bandwidth                       =   (slope * gMmwMssMCB.profileComCfg.h_NumOfAdcSamples)/(gMmwMssMCB.adcSamplingRate * 1.e6);
    centerFreq                      =   startFreq + bandwidth * 0.5f + adcStart * slope;

    params->rangeStep            =   (MMWDEMO_RFPARSER_SPEED_OF_LIGHT_IN_METERS_PER_SEC * (gMmwMssMCB.adcSamplingRate * 1.e6)) /
                                        (2.f * slope * (2*params->numRangeBins)); //ToDo: Check this
    /*outParams->rangeResolution      =   (MMWDEMO_RFPARSER_SPEED_OF_LIGHT_IN_METERS_PER_SEC * gMmwMssMCB.adcSamplingRate * 1e6) /
                                        (2.f * slope * gMmwMssMCB.profileComCfg.h_NumOfAdcSamples);*/

    if (gMmwMssMCB.frameCfg.h_NumOfBurstsInFrame > 1)
    {
        /* Burst mode: h_NumOfBurstsInFrame > 1, h_NumOfChirpsInBurst = 2 */
        params->dopplerStep          =   MMWDEMO_RFPARSER_SPEED_OF_LIGHT_IN_METERS_PER_SEC /
                                            (2.f * params->numDopplerBins *
                                            centerFreq * (gMmwMssMCB.burstPeriod * 1e-6));
    }
    else
    {
        /* Normal mode: h_NumOfBurstsInFrame = 1, h_NumOfChirpsInBurst >= 2 */
        params->dopplerStep          =   MMWDEMO_RFPARSER_SPEED_OF_LIGHT_IN_METERS_PER_SEC /
                                            (2.f * gMmwMssMCB.frameCfg.h_NumOfChirpsInBurst *
                                            centerFreq * ((gMmwMssMCB.profileTimeCfg.h_ChirpIdleTime + gMmwMssMCB.profileComCfg.h_ChirpRampEndTime) * 1e-1 * 1e-6));
    }
    /*outParams->dopplerResolution    =   MMWDEMO_RFPARSER_SPEED_OF_LIGHT_IN_METERS_PER_SEC /
                                        (2.f * gMmwMssMCB.frameCfg.h_NumOfBurstsInFrame * centerFreq * (gMmwMssMCB.frameCfg.w_BurstPeriodicity));*/

    if (gMmwMssMCB.antennaGeometryCfg.antDistanceXdim == 0.)
    {
        params->lambdaOverDistX = 2.0;
    }
    else
    {
        params->lambdaOverDistX = 3e8 / (centerFreq * gMmwMssMCB.antennaGeometryCfg.antDistanceXdim);
    }

    if (gMmwMssMCB.antennaGeometryCfg.antDistanceZdim == 0.)
    {
        params->lambdaOverDistZ = 2.0;
    }
    else
    {
        params->lambdaOverDistZ = 3e8 / (centerFreq * gMmwMssMCB.antennaGeometryCfg.antDistanceZdim);
    }

    /* Range bias (m) */
    params->rangeBias = gMmwMssMCB.compRxChannelBiasCfg.rangeBias;

    if(gMmwMssMCB.adcDataSourceCfg.source == 1)
    {
        //ADC data from file, populate point cloud list with target indices (range/azimuth/elevation/doppler)
        params->enableCfarPointCloudListWithIndices = true;
    }
    else
    {
        params->enableCfarPointCloudListWithIndices = false;
    }

    /* hwres config - Copy these structures */
    pHwConfig->dopplerIndexMatrix = gMmwMssMCB.dopplerIndexMatrix;
    pHwConfig->elevationIndexMatrix = gMmwMssMCB.elevationIndexMatrix;

    /* For point cloud compression over UART: set the cloud point units and reciprocal values */
    gMmwMssMCB.pointCloudToUart.pointUint.xyzUnit = (params->rangeStep * params->numRangeBins) / 32768.0;
    gMmwMssMCB.pointCloudToUart.pointUint.dopplerUnit = (params->dopplerStep * params->numDopplerBins/2) / 32768.0;
    gMmwMssMCB.pointCloudToUart.pointUint.snrUint = 0.25;
    gMmwMssMCB.pointCloudToUart.pointUint.noiseUint = 1.0;
    gMmwMssMCB.pointCloudUintRecip.xyzUnit = 1. / gMmwMssMCB.pointCloudToUart.pointUint.xyzUnit;
    gMmwMssMCB.pointCloudUintRecip.dopplerUnit = 1. / gMmwMssMCB.pointCloudToUart.pointUint.dopplerUnit;
    gMmwMssMCB.pointCloudUintRecip.snrUint = 1. / gMmwMssMCB.pointCloudToUart.pointUint.snrUint;  //scale 0.1 since in the CFAR DPU structure it is in 0.1 dB
    gMmwMssMCB.pointCloudUintRecip.noiseUint = 1. / gMmwMssMCB.pointCloudToUart.pointUint.noiseUint;  //scale 0.1 since in the CFAR DPU structure it is in 0.1 dB


    if(gMmwMssMCB.enableMajorMotion)
    {
        /* In major motion CFAR post processing, the velocity inclusion threshold is set to maximum, so all points are includeed and their detected velocity is not modified */
        gMmwMssMCB.cfarRunTimeInputParams[0].forceVelocityToZero = false;
        gMmwMssMCB.cfarRunTimeInputParams[0].velocityInclusionThr = params->dopplerStep * params->numDopplerBins/2; //Maximum velocity, include all
    }
    if(gMmwMssMCB.enableMinorMotion)
    {
        /* In minor motion CFAR post processing, the detected points with radial velocity greater than minorMotionVelocityInclusionThr are excluded from the list, 
           and then velocity  of included points is forced to zero if the flag forceMinorMotionVelocityToZero is set to true. */
        gMmwMssMCB.cfarRunTimeInputParams[1].forceVelocityToZero = gMmwMssMCB.sigProcChainCfg.forceMinorMotionVelocityToZero;
        gMmwMssMCB.cfarRunTimeInputParams[1].velocityInclusionThr = gMmwMssMCB.sigProcChainCfg.minorMotionVelocityInclusionThr;
    }

    return retVal;
}

/**
*  @b Description
*  @n
*    Based on the configuration, set up the micro Doppler processing DPU configurations
*/
int32_t uDopProc_setProfile()
{
    /* Doaproc DPU */
    DPU_uDopProc_EdmaCfg *edmaCfg;
    DPU_uDopProc_HwaCfg *hwaCfg;
    int32_t retVal = 0;
    DPU_uDopProc_StaticConfig  *uDopStaticCfg;
    DPU_uDopProc_HW_Resources  *hwRes;
    float adcStart, startFreq, slope, bandwidth, centerFreq;

    memset((void *)&uDopProcDpuCfg, 0, sizeof(uDopProcDpuCfg));

    if (!gMmwMssMCB.oneTimeConfigDone)
    {
        uDopProcDpuCfg.isFirstTimeCfg = true;
    }
    else
    {
        uDopProcDpuCfg.isFirstTimeCfg = false;
    }

    hwRes = &uDopProcDpuCfg.hwRes;
    uDopStaticCfg = &uDopProcDpuCfg.staticCfg;
    edmaCfg = &hwRes->edmaCfg;
    hwaCfg = &hwRes->hwaCfg;

    if (!(gMmwMssMCB.trackerCfg.staticCfg.trackerEnabled & gMmwMssMCB.enableMajorMotion))
    {
        /*Requires tracker and major motion to be enabled.*/
        retVal = DPC_OBJECTDETECTION_EINVAL_CFG;
        goto exit;
    }
    uDopStaticCfg->numAntRow = gMmwMssMCB.numAntRow;
    uDopStaticCfg->numAntCol = gMmwMssMCB.numAntCol;
    uDopStaticCfg->angleDimension = gMmwMssMCB.angleDimension;

    uDopStaticCfg->numDopplerChirps   = gMmwMssMCB.frameCfg.h_NumOfBurstsInFrame * gMmwMssMCB.frameCfg.h_NumOfChirpsInBurst / gMmwMssMCB.numTxAntennas;
    uDopStaticCfg->numDopplerBins     = mathUtils_pow2roundup(uDopStaticCfg->numDopplerChirps);
    uDopStaticCfg->numRangeBins       = gMmwMssMCB.numRangeBins;
    uDopStaticCfg->numRxAntennas      = gMmwMssMCB.numRxAntennas;
    uDopStaticCfg->numVirtualAntennas = gMmwMssMCB.numRxAntennas * gMmwMssMCB.numTxAntennas;
    uDopStaticCfg->log2NumDopplerBins = mathUtils_ceilLog2(uDopStaticCfg->numDopplerBins);;
    uDopStaticCfg->numTxAntennas      = gMmwMssMCB.numTxAntennas;
    uDopStaticCfg->maxNumTracks       = gMmwMssMCB.trackerCfg.staticCfg.gtrackModuleConfig.maxNumTracks;

    adcStart                        =   (gMmwMssMCB.adcStartTime * 1.e-6);
    startFreq                       =   (float)(gMmwMssMCB.startFreq * 1.e9);
    slope                           =   (float)(gMmwMssMCB.chirpSlope * 1.e12);
    bandwidth                       =   (slope * gMmwMssMCB.profileComCfg.h_NumOfAdcSamples)/(gMmwMssMCB.adcSamplingRate * 1.e6);
    centerFreq                      =   startFreq + bandwidth * 0.5f + adcStart * slope;

    uDopStaticCfg->rangeStep          = (MMWDEMO_RFPARSER_SPEED_OF_LIGHT_IN_METERS_PER_SEC * (gMmwMssMCB.adcSamplingRate * 1.e6)) /
                                        (2.f * slope * (2*gMmwMssMCB.numRangeBins));

    if (gMmwMssMCB.frameCfg.h_NumOfBurstsInFrame > 1)
    {
        /* Burst mode: h_NumOfBurstsInFrame > 1, h_NumOfChirpsInBurst = 2 */
        uDopStaticCfg->dopplerStep          =   MMWDEMO_RFPARSER_SPEED_OF_LIGHT_IN_METERS_PER_SEC /
                                            (2.f * uDopStaticCfg->numDopplerBins *
                                            centerFreq * (gMmwMssMCB.burstPeriod * 1e-6));
    }
    else
    {
        /* Normal mode: h_NumOfBurstsInFrame = 1, h_NumOfChirpsInBurst >= 2 */
        uDopStaticCfg->dopplerStep          =   MMWDEMO_RFPARSER_SPEED_OF_LIGHT_IN_METERS_PER_SEC /
                                            (2.f * gMmwMssMCB.frameCfg.h_NumOfChirpsInBurst *
                                            centerFreq * ((gMmwMssMCB.profileTimeCfg.h_ChirpIdleTime + gMmwMssMCB.profileComCfg.h_ChirpRampEndTime) * 1e-1 * 1e-6));
    }

    uDopStaticCfg->angleDimension        = gMmwMssMCB.angleDimension;
    uDopStaticCfg->isDetMatrixLogScale   = false;
    uDopStaticCfg->azimuthFftSize        = gMmwMssMCB.sigProcChainCfg.azimuthFftSize;
    uDopStaticCfg->elevationFftSize        = gMmwMssMCB.sigProcChainCfg.elevationFftSize;
    uDopStaticCfg->isStaticClutterRemovalEnabled = gMmwMssMCB.staticClutterRemovalEnable;
    uDopStaticCfg->isRxChGainPhaseCompensationEnabled   = true;

    /* CLI configuration */
    uDopStaticCfg->cliCfg = gMmwMssMCB.microDopplerCliCfg;
    uDopStaticCfg->microDopplerClassifierCliCfg  = gMmwMssMCB.microDopplerClassifierCliCfg;

    uDopStaticCfg->maxNumAzimAccumBins = DPU_UDOPPROC_MAX_NUM_AZIMUTH_ACCUM_BINS;

    /* hwRes - copy these structures */
    hwRes->radarCube = gMmwMssMCB.radarCube[0];   //ToDo remove from here it is passed through the process function, set to NULL

    /* hwRes - micro Doppler output array */
    hwRes->uDopplerHwaOutput = DPC_ObjDet_MemPoolAlloc(&gMmwMssMCB.CoreLocalRamObj,
                                                       uDopStaticCfg->numDopplerBins * sizeof(uint32_t) * 2, //allocate 2 buffers (for ping/pong) ToDo allocate as scratch
                                                       sizeof(uint32_t));
    if (hwRes->uDopplerHwaOutput == NULL)
    {
        retVal = DPC_OBJECTDETECTION_ENOMEM__MICRO_DOPPLER_BUFFER;
        goto exit;
    }

    gMmwMssMCB.uDopProcOutParams.uDopplerOutput = (float *) DPC_ObjDet_MemPoolAlloc(&gMmwMssMCB.CoreLocalRamObj,
                                                                                    uDopStaticCfg->numDopplerBins * sizeof(float) * TRACKER_MAX_NUM_TR,
                                                                                    sizeof(float));
    if (gMmwMssMCB.uDopProcOutParams.uDopplerOutput == NULL)
    {
        retVal = DPC_OBJECTDETECTION_ENOMEM__MICRO_DOPPLER_BUFFER;
        goto exit;
    }

    gMmwMssMCB.uDopProcOutParams.uDopplerFeatures = (FeatExtract_featOutput *) DPC_ObjDet_MemPoolAlloc(&gMmwMssMCB.CoreLocalRamObj,
                                                                                                       sizeof(FeatExtract_featOutput) * TRACKER_MAX_NUM_TR,
                                                                                                       sizeof(float));
    if (gMmwMssMCB.uDopProcOutParams.uDopplerFeatures == NULL)
    {
        retVal = DPC_OBJECTDETECTION_ENOMEM__MICRO_DOPPLER_FEATURES;
        goto exit;
    }


    /* hwRes - edmaCfg */
    edmaCfg->edmaHandle = gEdmaHandle[0];

    /* edma to reset accumulating micro doppler per range bain */
    edmaCfg->edmaResetIn.channel =           DPC_OBJDET_DPU_UDOP_PROC_EDMA_RESET_CH;
    edmaCfg->edmaResetIn.channelShadow =     DPC_OBJDET_DPU_UDOP_PROC_EDMA_RESET_SHADOW;
    edmaCfg->edmaResetIn.eventQueue =        DPC_OBJDET_DPU_UDOP_PROC_EDMARESET_EVENT_QUE;

    /* edmaIn - to transfer one range bin from radar cube matrix to HWA */
    edmaCfg->edmaIn.channel =           DPC_OBJDET_DPU_UDOP_PROC_EDMAIN_CH;
    edmaCfg->edmaIn.channelShadow =     DPC_OBJDET_DPU_UDOP_PROC_EDMAIN_SHADOW;
    edmaCfg->edmaIn.eventQueue =        DPC_OBJDET_DPU_UDOP_PROC_EDMAIN_EVENT_QUE;

    /* edmaHotSig */
    edmaCfg->edmaHotSig.channel =             DPC_OBJDET_DPU_UDOP_PROC_EDMAIN_SIG_CH;
    edmaCfg->edmaHotSig.channelShadow =       DPC_OBJDET_DPU_UDOP_PROC_EDMAIN_SIG_SHADOW;
    edmaCfg->edmaHotSig.eventQueue =          DPC_OBJDET_DPU_UDOP_PROC_EDMAIN_SIG_EVENT_QUE;

    /* edmaChainOut - loop back and in last iteration chain to output buffer */
    edmaCfg->edmaChainOut.channel =           DPC_OBJDET_DPU_UDOP_PROC_EDMAOUT_CHAIN_CH;
    edmaCfg->edmaChainOut.ShadowPramId[0] =  DPC_OBJDET_DPU_UDOP_PROC_EDMAOUT_CHAIN0_SHADOW;
    edmaCfg->edmaChainOut.ShadowPramId[1] =  DPC_OBJDET_DPU_UDOP_PROC_EDMAOUT_CHAIN1_SHADOW;
    edmaCfg->edmaChainOut.eventQueue =        DPC_OBJDET_DPU_UDOP_PROC_EDMAOUT_CHAIN_EVENT_QUE;

    /* edmaOut - transfer Micro Doppler from HWA to output ping/pong buffer */
    edmaCfg->edmaMicroDopOut.channel =       DPC_OBJDET_DPU_UDOP_PROC_EDMAOUT_UDOPPLER_CH;
    edmaCfg->edmaMicroDopOut.channelShadow = DPC_OBJDET_DPU_UDOP_PROC_EDMAOUT_UDOPPLER_SHADOW;
    edmaCfg->edmaMicroDopOut.eventQueue =    DPC_OBJDET_DPU_UDOP_PROC_EDMAOUT_UDOPPLER_EVENT_QUE;

    edmaCfg->intrObj = &intrObj_uDopProc;

    hwaCfg->paramSetStartIdx = DPU_RANGEPROCHWA_NUM_HWA_PARAM_SETS +
                               DPU_DOAPROC_MAX_NUM_HWA_PARAMSET +
                               DPU_CFARPROCHWA_MAX_NUM_HWA_PARAMSET;
    hwaCfg->dmaTrigSrcChan = DPC_ObjDet_HwaDmaTrigSrcChanPoolAlloc(&gMmwMssMCB.HwaDmaChanPoolObj);

    /* Rx compensation coefficients - ToDo this is shared with DOA DPU*/
    /* Rx compensation coefficients */
    {
        int32_t rxInd, txInd;
        int32_t ind = 0;
        uDopStaticCfg->compRxChanCfg.rangeBias = gMmwMssMCB.compRxChannelBiasCfg.rangeBias;
        for (txInd = 0; txInd < uDopStaticCfg->numTxAntennas; txInd++)
        {
            for (rxInd = 0; rxInd < uDopStaticCfg->numRxAntennas; rxInd++)
            {
                uDopStaticCfg->compRxChanCfg.rxChPhaseComp[ind++] = gMmwMssMCB.compRxChannelBiasCfg.rxChPhaseComp[gMmwMssMCB.rxAntOrder[rxInd] + (txInd * SYS_COMMON_NUM_RX_CHANNEL)]; //TODO: staticCfg->txAntOrder[txInd] * SYS_COMMON_NUM_RX_CHANNEL
            }
        }
    }

    /* Calculate Doppler+mapping table */
    retVal = MmwDemo_cfgDopplerParamMapping(&hwRes->doaRngGateCfg, DOPPLER_OUTPUT_MAPPING_ROW_DOP_COL);
    if (retVal < 0)
    {
        goto exit;
    }

    /* ToDo: Use scratch buffer for feature extraction */
    hwRes->scratchBuf.sizeBytes = (uDopStaticCfg->numDopplerBins + 1) * sizeof(float);

    if (uDopStaticCfg->microDopplerClassifierCliCfg.enabled)
    {
        /* Classifier on the target*/
        uint32_t classifierScratchBuffSizeBytes = classifier_bytes_needed();
        if (classifierScratchBuffSizeBytes > hwRes->scratchBuf.sizeBytes)
        {
            hwRes->scratchBuf.sizeBytes = classifierScratchBuffSizeBytes;
        }
    }


    //Shared between feature extraction and classifier
    hwRes->scratchBuf.data = DPC_ObjDet_MemPoolAlloc(&gMmwMssMCB.CoreLocalRamObj,
                                                     hwRes->scratchBuf.sizeBytes,
                                                     sizeof(float));
    if (hwRes->scratchBuf.data == NULL)
    {
        retVal = DPC_OBJECTDETECTION_ENOMEM__MICRO_DOPPLER_FEATURES;
        goto exit;
    }

    if (uDopStaticCfg->microDopplerClassifierCliCfg.enabled)
    {
        /* Memory for feature delay line - retained acreoss frames */
        hwRes->featureObj = DPC_ObjDet_MemPoolAlloc(&gMmwMssMCB.CoreLocalRamObj,
                                                    sizeof(DPU_uDopProc_FeatureObj) * gMmwMssMCB.trackerCfg.staticCfg.gtrackModuleConfig.maxNumTracks,
                                                    sizeof(float));
        if (hwRes->featureObj == NULL)
        {
            retVal = DPC_OBJECTDETECTION_ENOMEM__MICRO_DOPPLER_FEATURES;
            goto exit;
        }

        /* Scratch buffer for linearized feature set, input to classifier */
        hwRes->scratchBuf2.sizeBytes = CLASSIFIER_NUM_FRAMES * CLASSIFIER_NUM_FEATURES * sizeof(float);
        hwRes->scratchBuf2.data = DPC_ObjDet_MemPoolAlloc(&gMmwMssMCB.CoreLocalRamObj,
                                                          hwRes->scratchBuf2.sizeBytes,
                                                          sizeof(float));
        if (hwRes->scratchBuf2.data == NULL)
        {
            retVal = DPC_OBJECTDETECTION_ENOMEM__MICRO_DOPPLER_FEATURES;
            goto exit;
        }
    }

exit:
return retVal;
}

/**
*  @b Description
*  @n
*    Based on the configuration, set up the MPD processing DPU configurations
*/
int32_t MpdProc_setProfile()
{
    int32_t retVal = 0;
    int16_t i = 0;
    DPU_MpdProc_HW_Resources *pHwConfig;
    DPU_MpdProc_StaticConfig *params;

    /* CFARproc DPU */
    pHwConfig = &mpdProcDpuCfg.res;
    params = &mpdProcDpuCfg.staticCfg;

    memset((void *)&mpdProcDpuCfg, 0, sizeof(DPU_MpdProc_Config));
    
    pHwConfig->zones = gMmwMssMCB.dpcZones;

    /* Initializing the zones structure based on CLI config. CAUTION: Accumulates history across frames */
    if (!gMmwMssMCB.oneTimeConfigDone)
    {
        memset((void *)pHwConfig->zones, 0, (gMmwMssMCB.sceneryParams.numBoundaryBoxes * sizeof(mpdProc_MotionTracker)));

        for(i = 0;i < gMmwMssMCB.sceneryParams.numBoundaryBoxes;i++)
        {
            pHwConfig->zones[i].pointHistBufferMajor.bufferSize = gMmwMssMCB.majorStateParamCfg.histBufferSize;
            pHwConfig->zones[i].pointHistBufferMajor.oldest = gMmwMssMCB.majorStateParamCfg.histBufferSize - 1;
            
            pHwConfig->zones[i].snrHistBufferMajor.bufferSize = gMmwMssMCB.majorStateParamCfg.histBufferSize;
            pHwConfig->zones[i].snrHistBufferMajor.oldest = gMmwMssMCB.majorStateParamCfg.histBufferSize - 1;
            
            pHwConfig->zones[i].pointHistBufferMinor.bufferSize = gMmwMssMCB.minorStateParamCfg.histBufferSize;
            pHwConfig->zones[i].pointHistBufferMinor.oldest = gMmwMssMCB.minorStateParamCfg.histBufferSize - 1;

            pHwConfig->zones[i].snrHistBufferMinor.bufferSize = gMmwMssMCB.minorStateParamCfg.histBufferSize;
            pHwConfig->zones[i].snrHistBufferMinor.oldest = gMmwMssMCB.minorStateParamCfg.histBufferSize - 1;
        }
    }

    pHwConfig->numDetMajor = (uint16_t *)&gMmwMssMCB.dpcResult.numObjOutMajor;
    pHwConfig->numDetMinor = (uint16_t *)&gMmwMssMCB.dpcResult.numObjOutMinor;
    pHwConfig->detObjMajor = (DPIF_PointCloudCartesianExt *)&gMmwMssMCB.cfarDetObjOut[0];
    pHwConfig->detObjMinor = (DPIF_PointCloudCartesianExt *)&gMmwMssMCB.cfarDetObjOut[0];
    
    params->motionMode = (uint8_t)gMmwMssMCB.sigProcChainCfg.motDetMode;
    
    memcpy(&params->sceneryParams, &gMmwMssMCB.sceneryParams, sizeof(mpdProc_SceneryParams));
    memcpy(&params->clusterParams, &gMmwMssMCB.clusterParamCfg, sizeof(mpdProc_ClusterParamCfg));
    memcpy(&params->majorStateParamCfg, &gMmwMssMCB.majorStateParamCfg, sizeof(mpdProc_MotionModeStateParamCfg));
    memcpy(&params->minorStateParamCfg, &gMmwMssMCB.minorStateParamCfg, sizeof(mpdProc_MotionModeStateParamCfg));

    return retVal;
}

/**
*  @b Description
*  @n
*    Range processing DPU Initialization
*/
void rangeProc_dpuInit()
{
    int32_t errorCode = 0;
    DPU_RangeProcHWA_InitParams initParams;
    initParams.hwaHandle = hwaHandle;

    /* generate the dpu handler*/
    gMmwMssMCB.rangeProcDpuHandle = DPU_RangeProcHWA_init(&initParams, &errorCode);
    if (gMmwMssMCB.rangeProcDpuHandle == NULL)
    {
        CLI_write("Error: RangeProc DPU initialization returned error %d\n", errorCode);
        DebugP_assert(0);
        return;
    }
}

/**
*  @b Description
*  @n
*    DOA DPU Initialization
*/
void doaProc_dpuInit()
{
    int32_t  errorCode = 0;
    DPU_DoaProc_InitParams initParams;
    initParams.hwaHandle =  hwaHandle;
    /* generate the dpu handler*/
    gMmwMssMCB.doaProcDpuHandle =  DPU_DoaProc_init(&initParams, &errorCode);
    if (gMmwMssMCB.doaProcDpuHandle == NULL)
    {
        CLI_write ("Error: DoaProc DPU initialization returned error %d\n", errorCode);
        DebugP_assert (0);
        return;
    }
}

/**
*  @b Description
*  @n
*    Micro-Doppler DPU Initialization
*/
void udopProc_dpuInit()
{
    int32_t  errorCode = 0;
    DPU_uDopProc_InitParams initParams;
    initParams.hwaHandle =  hwaHandle;
    /* generate the dpu handler*/
    gMmwMssMCB.microDopDpuHandle =  DPU_uDopProc_init(&initParams, &errorCode);
    if (gMmwMssMCB.microDopDpuHandle == NULL)
    {
        CLI_write ("Error: Micro-Doppler Proc DPU initialization returned error %d\n", errorCode);
        DebugP_assert (0);
        return;
    }
}


/**
*  @b Description
*  @n
*    CFAR DPU Initialization
*/
void cfarProc_dpuInit()
{
    int32_t  errorCode = 0;
    DPU_CFARProcHWA_InitParams initParams;
    initParams.hwaHandle =  hwaHandle;
    /* generate the dpu handler*/
    gMmwMssMCB.cfarProcDpuHandle =  DPU_CFARProcHWA_init(&initParams, &errorCode);
    if (gMmwMssMCB.cfarProcDpuHandle == NULL)
    {
        CLI_write ("Error: CFAR Proc DPU initialization returned error %d\n", errorCode);
        DebugP_assert (0);
        return;
    }
}

/**
*  @b Description
*  @n
*    MPD DPU Initialization
*/
void mpdProc_dpuInit()
{
    int32_t  errorCode = 0;

    if (!gMmwMssMCB.oneTimeConfigDone)
    {
        gMmwMssMCB.sceneryParams.numBoundaryBoxes = 0;
    }
    
    /* generate the dpu handler*/
    gMmwMssMCB.mpdProcDpuHandle =  DPU_MpdProc_init(&errorCode);
    if (gMmwMssMCB.mpdProcDpuHandle == NULL)
    {
        CLI_write("Error: MPD Proc DPU initialization returned error %d\n", errorCode);
        DebugP_assert (0);
        return;
    }
}

/**
*  @b Description
*  @n
*    Processing chain initialization
*/
void trackerProc_dpuInit()
{
    int32_t  errorCode = 0;
    /* generate the dpu handler*/
    gMmwMssMCB.trackerProcDpuHandle =  DPU_TrackerProc_init(&errorCode);
    if (gMmwMssMCB.trackerProcDpuHandle == NULL)
    {
        CLI_write ("Error: Tracker Proc DPU initialization returned error %d\n", errorCode);
        DebugP_assert (0);
        return;
    }
}
void DPC_Init()
{
    /* hwa, edma, and DPU initialization*/

    /* Register Frame Start Interrupt */
    if(mmwDemo_registerFrameStartInterrupt() != 0){
        CLI_write("Error: Failed to register frame start interrupts\n");
        DebugP_assert(0);
    }
#if 0
    if(mmwDemo_registerChirpAvailableInterrupts() != 0){
        CLI_write("Failed to register chirp available interrupts\n");
        DebugP_assert(0);
    }
    mmwDemo_registerChirpInterrupt();
    mmwDemo_registerBurstInterrupt();
#endif
    int32_t status = SystemP_SUCCESS;

    /* Shared memory pool */
    gMmwMssMCB.L3RamObj.cfg.addr = (void *)&gMmwL3[0];
    gMmwMssMCB.L3RamObj.cfg.size = sizeof(gMmwL3);

    /* Local memory pool */
    gMmwMssMCB.CoreLocalRamObj.cfg.addr = (void *)&gMmwCoreLocMem[0];
    gMmwMssMCB.CoreLocalRamObj.cfg.size = sizeof(gMmwCoreLocMem);

    /* Memory pool for the tracker */
    HeapP_construct(&gMmwMssMCB.CoreLocalTrackerHeapObj, (void *) gMmwCoreLocMem2, MMWDEMO_OBJDET_CORE_LOCAL_MEM2_SIZE);

    /* Memory pool for the feature extraction */
    featExtract_heapConstruct();

    hwaHandle = HWA_open(0, NULL, &status);
    if (hwaHandle == NULL)
    {
        CLI_write("Error: Unable to open the HWA Instance err:%d\n", status);
        DebugP_assert(0);
    }

    rangeProc_dpuInit();
    doaProc_dpuInit();
    cfarProc_dpuInit();
    mpdProc_dpuInit();

    if (!gMmwMssMCB.oneTimeConfigDone)
    {
    	udopProc_dpuInit();
        trackerProc_dpuInit();
    }

}

/**
*  @b Description
*  @n
*        Function configuring range processing DPU
*/
void mmwDemo_rangeProcConfig()
{
    int32_t retVal = 0;

    retVal = RangeProc_setProfile();
    if (retVal < 0)
    {
        CLI_write("Error in setting up range profile:%d \n", retVal);
        DebugP_assert(0);
    }

    retVal = DPU_RangeProcHWA_config(gMmwMssMCB.rangeProcDpuHandle, &rangeProcDpuCfg);
    if (retVal < 0)
    {
        CLI_write("Error: RANGE DPU config return error:%d \n", retVal);
        DebugP_assert(0);
    }
}

/**
*  @b Description
*  @n
*        Function configuring DOA DPU
*/
void mmwDemo_doaProcConfig()
{
    //uint32_t i;
    int32_t retVal = 0;
    uint32_t numSamples;
    DPU_DoaProc_HW_Resources  *hwRes;

    hwRes = &doaProcDpuCfg.hwRes;

    retVal = DoaProc_setProfile();
    if (retVal < 0)
    {
        CLI_write("Error: Error in setting up doa profile:%d \n", retVal);
        DebugP_assert(0);
    }

    /* Note: chunk[1] is not used */
    /* Major motion - set input source and destination address */
    numSamples = doaProcDpuCfg.staticCfg.numVirtualAntennas * doaProcDpuCfg.staticCfg.numDopplerChirps;
    gMmwMssMCB.radarCubeSrc[MMW_DEMO_MAJOR_MODE].chunk[0].srcAddress = (uint32_t) gMmwMssMCB.radarCube[0].data;
    gMmwMssMCB.radarCubeSrc[MMW_DEMO_MAJOR_MODE].chunk[0].dstAddress = hwRes->hwaCfg.hwaMemInpAddr;
    gMmwMssMCB.radarCubeSrc[MMW_DEMO_MAJOR_MODE].chunk[0].Bcnt_Acnt = (numSamples << 16)   | sizeof(cmplx16ImRe_t);

    /* Minor Motion set input source and destination address */
    numSamples = doaProcDpuCfg.staticCfg.numVirtualAntennas * doaProcDpuCfg.staticCfg.numDopplerChirps;
    gMmwMssMCB.radarCubeSrc[MMW_DEMO_MINOR_MODE].chunk[0].srcAddress = (uint32_t) gMmwMssMCB.radarCube[1].data;
    gMmwMssMCB.radarCubeSrc[MMW_DEMO_MINOR_MODE].chunk[0].dstAddress = hwRes->hwaCfg.hwaMemInpAddr;
    gMmwMssMCB.radarCubeSrc[MMW_DEMO_MINOR_MODE].chunk[0].Bcnt_Acnt = (numSamples << 16)   | sizeof(cmplx16ImRe_t);

    retVal = DPU_DoaProc_config (gMmwMssMCB.doaProcDpuHandle, &doaProcDpuCfg);
    if (retVal < 0)
    {
        CLI_write("DOA DPU config return error:%d \n", retVal);
        DebugP_assert(0);
    }

}

/**
 *  @b Description
 *  @n
*        Function configuring Micro Doppler DPU
*/
void mmwDemo_uDopProcConfig()
{
    int32_t retVal = 0;
    retVal = uDopProc_setProfile();
    if (retVal < 0)
    {
        CLI_write("Error in setting up micro Doppler profile:%d \n", retVal);
        DebugP_assert(0);
    }

    retVal = DPU_uDopProc_config(gMmwMssMCB.microDopDpuHandle, &uDopProcDpuCfg);
    if (retVal < 0)
    {
        CLI_write("Micro Doppler DPU config return error:%d \n", retVal);
        DebugP_assert(0);
    }
}

/**
*  @b Description
*  @n
*        Function configuring CFAR DPU
*/
void mmwDemo_cfarProcConfig()
{
    int32_t retVal = 0;

    retVal = CfarProc_setProfile();
    if (retVal < 0)
    {
        CLI_write("Error in setting up CFAR profile:%d \n", retVal);
        DebugP_assert(0);
    }

    retVal = DPU_CFARProcHWA_config(gMmwMssMCB.cfarProcDpuHandle, &cfarProcDpuCfg);
    if (retVal < 0)
    {
        CLI_write("CFAR DPU config return error:%d \n", retVal);
        DebugP_assert(0);
    }
}

/**
 *  @b Description
 *  @n
*        Function configuring MPD DPU
*/
void mmwDemo_mpdProcConfig()
{
    int32_t retVal = 0;
    retVal = MpdProc_setProfile(); //TODO: Define function - cover motionTrackerInit() implementation
    if (retVal < 0)
    {
        CLI_write("Error in setting up MPD profile:%d \n", retVal);
        DebugP_assert(0);
    }

    retVal = DPU_MpdProc_config(gMmwMssMCB.mpdProcDpuHandle, &mpdProcDpuCfg);
    if (retVal < 0)
    {
        CLI_write("MPD DPU config return error:%d \n", retVal);
        DebugP_assert(0);
    }
}

/**
*  @b Description
*  @n
 *      The function copies sensor position configuration to tracker configuration structure
 *
 */
void MmwDemo_FillTrackerSensorPositionCfg()
{

    /*populate sensor position configuration*/
    memcpy(&gMmwMssMCB.trackerCfg.staticCfg.sceneryParams.sensorPosition, &gMmwMssMCB.sceneryParams.sensorPosition, sizeof(GTRACK_sensorPosition));

    /*populate sensor orientation configuration*/
    memcpy(&gMmwMssMCB.trackerCfg.staticCfg.sceneryParams.sensorOrientation, &gMmwMssMCB.sceneryParams.sensorOrientation, sizeof(GTRACK_sensorOrientation));

    /*demo parameters*/
    gMmwMssMCB.trackerCfg.staticCfg.sensorAzimuthTilt = gMmwMssMCB.trackerCfg.staticCfg.sceneryParams.sensorOrientation.azimTilt * 3.1415926f / 180.;
    gMmwMssMCB.trackerCfg.staticCfg.sensorElevationTilt = gMmwMssMCB.trackerCfg.staticCfg.sceneryParams.sensorOrientation.elevTilt * 3.1415926f / 180.;
    gMmwMssMCB.trackerCfg.staticCfg.sensorHeight = gMmwMssMCB.trackerCfg.staticCfg.sceneryParams.sensorPosition.z;

}



/**
*  @b Description
*  @n
 *      The function is used to configure the tracker DPU.
 *
 */
void mmwDemo_trackerConfig (void)
{
    int32_t    retVal=0;

    /* Fill sensor position */
    MmwDemo_FillTrackerSensorPositionCfg();

    retVal = DPU_TrackerProc_config(gMmwMssMCB.trackerProcDpuHandle, &gMmwMssMCB.trackerCfg);

    if (retVal < 0)
    {
        CLI_write("Tracker DPU config return error:%d \n", retVal);
        DebugP_assert(0);
    }
}

/**
*  @b Description
*  @n

*        Function configuring all DPUs
*/
void mmwDemo_DpcConfig()
{

    int32_t retVal;

    /*TODO Cleanup: MMWLPSDK-237*/
    
    DPC_ObjDet_MemPoolReset(&gMmwMssMCB.L3RamObj);
    DPC_ObjDet_MemPoolReset(&gMmwMssMCB.CoreLocalRamObj);
    DPC_ObjDet_HwaDmaTrigSrcChanPoolReset(&gMmwMssMCB.HwaDmaChanPoolObj);


    /*Allocate memory for point cloud */
    gMmwMssMCB.cfarDetObjOut = (DPIF_PointCloudCartesianExt *) DPC_ObjDet_MemPoolAlloc(&gMmwMssMCB.L3RamObj,
                                                                                       MAX_NUM_DETECTIONS * sizeof(DPIF_PointCloudCartesianExt),
                                                                                       sizeof(uint32_t));
    if (gMmwMssMCB.cfarDetObjOut == NULL)
    {
        CLI_write("DPC configuration: memory allocation failed\n");
        DebugP_assert(0);
    }

    gMmwMssMCB.dpcObjOut = (DPIF_PointCloudCartesian *) DPC_ObjDet_MemPoolAlloc(&gMmwMssMCB.L3RamObj,
                                                                                       MAX_NUM_DETECTIONS * sizeof(DPIF_PointCloudCartesian),
                                                                                       sizeof(uint32_t));
    if (gMmwMssMCB.dpcObjOut == NULL)
    {
        CLI_write("DPC configuration: memory allocation failed\n");
        DebugP_assert(0);
    }

    gMmwMssMCB.dpcObjSideInfo = (DPIF_PointCloudSideInfo *) DPC_ObjDet_MemPoolAlloc(&gMmwMssMCB.L3RamObj,
                                                                                       MAX_NUM_DETECTIONS * sizeof(DPIF_PointCloudSideInfo),
                                                                                       sizeof(uint32_t));
    if (gMmwMssMCB.dpcObjSideInfo == NULL)
    {
        CLI_write("DPC configuration: memory allocation failed\n");
        DebugP_assert(0);
    }

    if (gMmwMssMCB.adcDataSourceCfg.source == 1)
    {
        gMmwMssMCB.dpcObjIndOut = (DPIF_PointCloudRngAzimElevDopInd *) DPC_ObjDet_MemPoolAlloc(&gMmwMssMCB.L3RamObj,
                                                                                           MAX_NUM_DETECTIONS * sizeof(DPIF_PointCloudRngAzimElevDopInd),
                                                                                           sizeof(uint32_t));
        if (gMmwMssMCB.dpcObjIndOut == NULL)
        {
            CLI_write("DPC configuration: memory allocation failed\n");
            DebugP_assert(0);
        }
    }

    if(gMmwMssMCB.isMotionPresenceDpuEnabled)
    {
        /*Allocate memory for zone state array based on no of zones configured*/
        gMmwMssMCB.dpcZones = (mpdProc_MotionTracker *) DPC_ObjDet_MemPoolAlloc(&gMmwMssMCB.L3RamObj,
                                                                                    gMmwMssMCB.sceneryParams.numBoundaryBoxes * sizeof(mpdProc_MotionTracker),
                                                                                    sizeof(uint8_t));

        gMmwMssMCB.dpcZoneState = (uint8_t *) DPC_ObjDet_MemPoolAlloc(&gMmwMssMCB.L3RamObj,
                                                                        ((uint8_t)ceil(gMmwMssMCB.sceneryParams.numBoundaryBoxes/4.0) + 1) * sizeof(uint8_t),
                                                                        sizeof(uint8_t));
    }


    /* Configure DPUs */
    mmwDemo_rangeProcConfig();
    mmwDemo_doaProcConfig();

    if(gMmwMssMCB.microDopplerCliCfg.enabled)
    {
        if (!gMmwMssMCB.trackerCfg.staticCfg.trackerEnabled)
        {
            CLI_write("DPC configuration: Micro Doppler DPU requires group tracker DPU to be enabled \n");
            DebugP_assert(0);
        }

    	mmwDemo_uDopProcConfig();
    }

    mmwDemo_cfarProcConfig();

    if(gMmwMssMCB.isMotionPresenceDpuEnabled)
    {
        mmwDemo_mpdProcConfig();
    }

    if(gMmwMssMCB.trackerCfg.staticCfg.trackerEnabled)
    {
        if (!gMmwMssMCB.oneTimeConfigDone)
        {
            mmwDemo_trackerConfig();
        }
    }

    if(gMmwMssMCB.measureRxChannelBiasCliCfg.enabled)
    {
        retVal = mmwDemo_rangeBiasRxChPhaseMeasureConfig();
        if (retVal != 0)
        {
            CLI_write("DPC configuration: Invalid Rx channel compensation procedure configuration \n");
            DebugP_assert(0);
        }
    }

    if (!gMmwMssMCB.oneTimeConfigDone)
    {

        /* Report RAM usage */
        gMmwMssMCB.memUsage.CoreLocalRamUsage = DPC_ObjDet_MemPoolGetMaxUsage(&gMmwMssMCB.CoreLocalRamObj);
        gMmwMssMCB.memUsage.L3RamUsage = DPC_ObjDet_MemPoolGetMaxUsage(&gMmwMssMCB.L3RamObj);
        HeapP_getHeapStats(&gMmwMssMCB.CoreLocalTrackerHeapObj, &gMmwMssMCB.memUsage.trackerHeapStats);

        gMmwMssMCB.memUsage.L3RamTotal = gMmwMssMCB.L3RamObj.cfg.size;
        gMmwMssMCB.memUsage.CoreLocalRamTotal = gMmwMssMCB.CoreLocalRamObj.cfg.size;
    
        if(gMmwMssMCB.lowPowerMode == LOW_PWR_MODE_DISABLE)
        {
            DebugP_log(" ========== Memory Stats ==========\n");
            DebugP_log("%20s %12s %12s %12s\n", " ", "Size", "Used", "Free");

            DebugP_log("%20s %12d %12d %12d\n", "L3",
                      sizeof(gMmwL3),
                      gMmwMssMCB.memUsage.L3RamUsage,
                      sizeof(gMmwL3) - gMmwMssMCB.memUsage.L3RamUsage);

            DebugP_log("%20s %12d %12d %12d\n", "Local",
                      sizeof(gMmwCoreLocMem),
                      gMmwMssMCB.memUsage.CoreLocalRamUsage,
                      sizeof(gMmwCoreLocMem) - gMmwMssMCB.memUsage.CoreLocalRamUsage);
            DebugP_log("%20s %12d %12d %12d\n", "Tracker",
                      sizeof(gMmwCoreLocMem2),
                      sizeof(gMmwCoreLocMem2) - gMmwMssMCB.memUsage.trackerHeapStats.availableHeapSpaceInBytes,
                      gMmwMssMCB.memUsage.trackerHeapStats.availableHeapSpaceInBytes);
            DebugP_log("%20s %12d %12d %12d\n", "FeatExt",
                      sizeof(gMmwCoreLocMem3),
                      featExtract_memUsage(),
                      sizeof(gMmwCoreLocMem3) - featExtract_memUsage());
        }
    }

}

/**
 *  @b Description
 *  @n
 *     Compress point cloud list which is transferred to the Host via UART.
 *     Floating point values are converted to int16
 *
 * @param[out] pointCloudOut        Compressed point cloud list
 * @param[in]  pointCloudUintRecip  Scales used for conversion from float values to integer value
 * @param[in]  pointCloudIn         Input point cloud list, generated by CFAR DPU
 * @param[in]  numPoints            Number of points in the point cloud list
 *
 *  @retval
 *      Not Applicable.
 */
void MmwDemo_compressPointCloudList(MmwDemo_output_message_UARTpointCloud *pointCloudOut,
                                    MmwDemo_output_message_point_unit *pointCloudUintRecip,
                                    DPIF_PointCloudCartesianExt *pointCloudIn,
                                    uint32_t numPoints)
{
    uint32_t i;
    float xyzUnitScale = pointCloudUintRecip->xyzUnit;
    float dopplerScale = pointCloudUintRecip->dopplerUnit;
    float snrScale = pointCloudUintRecip->snrUint;
    float noiseScale = pointCloudUintRecip->noiseUint;
    uint32_t tempVal;

    for (i = 0; i < numPoints; i++)
    {
        pointCloudOut->point[i].x = (int16_t) roundf(pointCloudIn[i].x * xyzUnitScale); //ToDo saturate the values
        pointCloudOut->point[i].y = (int16_t) roundf(pointCloudIn[i].y * xyzUnitScale);
        pointCloudOut->point[i].z = (int16_t) roundf(pointCloudIn[i].z * xyzUnitScale);
        pointCloudOut->point[i].doppler = (int16_t) roundf(pointCloudIn[i].velocity * dopplerScale);
        tempVal = (uint32_t) roundf(pointCloudIn[i].snr * snrScale);
        if (tempVal > 255)
        {
            tempVal = 255;
        }
        pointCloudOut->point[i].snr = (uint8_t) tempVal;
        tempVal = (uint32_t) roundf(pointCloudIn[i].noise * noiseScale);
        if (tempVal > 255)
        {
            tempVal = 255;
        }
        pointCloudOut->point[i].noise = (uint8_t) tempVal;
    }
}

/**
 *  @b Description
 *  @n
 *     UART write wrapper function
 *
 * @param[in]   handle          UART handle
 * @param[in]   payload         Pointer to payload data
 * @param[in]   payloadLength   Payload length in bytes
 *
 *  @retval
 *      Not Applicable.
 */
void mmw_UartWrite (UART_Handle handle,
                    uint8_t *payload,
                    uint32_t payloadLength)
{
    UART_Transaction trans;

    UART_Transaction_init(&trans);

    trans.buf   = payload;
    trans.count = payloadLength;

    UART_write(handle, &trans);
}

#ifndef INA228
void mmwDemo_PM_Null(I2C_Handle i2cHandle, uint16_t *ptrPwrMeasured)
{
    ptrPwrMeasured[0] = (uint16_t)0xFFFF;
    ptrPwrMeasured[1] = (uint16_t)0xFFFF;
    ptrPwrMeasured[2] = (uint16_t)0xFFFF;
    ptrPwrMeasured[3] = (uint16_t)0xFFFF;
}
#endif


/** @brief Transmits detection data over UART
*
*    The following data is transmitted:
*    1. Header (size = 32bytes), including "Magic word", (size = 8 bytes)
*       and including the number of TLV items
*    TLV Items:
*    2. If pointCloud flag is 1 or 2, DPIF_PointCloudCartesian structure containing
*       X,Y,Z location and velocity for detected objects,
*       size = sizeof(DPIF_PointCloudCartesian) * number of detected objects
*    3. If pointCloud flag is 1, DPIF_PointCloudSideInfo structure containing SNR
*       and noise for detected objects,
*       size = sizeof(DPIF_PointCloudCartesian) * number of detected objects
*    4. If rangeProfile flag is set,  rangeProfile,
*       size = number of range bins * sizeof(uint32_t)
*    5. noiseProfile flag is set is not used.
*    6. If rangeAzimuthHeatMap flag is set, sends range/azimuth heatmap, size = number of range bins *
*       number of azimuth bins * sizeof(uint32_t)
*    7. rangeDopplerHeatMap flag is not used
*    8. If statsInfo flag is set, the stats information, timing, temperature and power
*/
void mmwDemo_TransmitProcessedOutputTask()
{
    UART_Handle uartHandle = gUartHandle[0];
    I2C_Handle  i2cHandle = gI2cHandle[CONFIG_I2C0];
    DPC_ObjectDetection_ExecuteResult *result = &gMmwMssMCB.dpcResult;
    //MmwDemo_output_message_stats      *timingInfo
    MmwDemo_output_message_header header;
    CLI_GuiMonSel   *pGuiMonSel;
    uint32_t tlvIdx = 0;
    //uint32_t i;
    uint32_t numPaddingBytes;
    uint32_t packetLen;
    uint8_t padding[MMWDEMO_OUTPUT_MSG_SEGMENT_LEN];
    MmwDemo_output_message_tl   tl[MMWDEMO_OUTPUT_ALL_MSG_MAX];
    uint8_t trackerEnabled;
    uint8_t microDopplerEnabled;
    uint8_t classifierEnabled;
    uint8_t mpdEnabled;
    uint32_t numTargets, numIndices, numDopplerBins;
    uint32_t numClassifiedTargets;
    uint8_t     *tList;
    uint8_t     *tIndex;

    uint8_t     *uDopplerData;
    uint8_t     *uDopplerFeatures;
    uint8_t     *uClassifierOutput;


    /* Get Gui Monitor configuration */
    pGuiMonSel = &gMmwMssMCB.guiMonSel; //&subFrameCfg->guiMonSel;

    trackerEnabled = gMmwMssMCB.trackerCfg.staticCfg.trackerEnabled;
    microDopplerEnabled = gMmwMssMCB.microDopplerCliCfg.enabled;
    classifierEnabled = gMmwMssMCB.microDopplerClassifierCliCfg.enabled;
    mpdEnabled = gMmwMssMCB.isMotionPresenceDpuEnabled;

    while(true)
    {
        SemaphoreP_pend(&gMmwMssMCB.tlvSemHandle, SystemP_WAIT_FOREVER);
        tlvIdx = 0;

        /* Clear message header */
        memset((void *)&header, 0, sizeof(MmwDemo_output_message_header));
        /* Header: */
        header.platform =  0xA6432;
        header.magicWord[0] = 0x0102;
        header.magicWord[1] = 0x0304;
        header.magicWord[2] = 0x0506;
        header.magicWord[3] = 0x0708;
        header.numDetectedObj = result->numObjOut;
        header.version =    MMWAVE_SDK_VERSION_BUILD |   //DEBUG_VERSION
                            (MMWAVE_SDK_VERSION_BUGFIX << 8) |
                            (MMWAVE_SDK_VERSION_MINOR << 16) |
                            (MMWAVE_SDK_VERSION_MAJOR << 24);

        /* Tracker information */
        numTargets = result->trackerOutParams.numTargets;
        numIndices = result->trackerOutParams.numIndices;
        tList      = (uint8_t*) result->trackerOutParams.tList;
        tIndex     = (uint8_t*) result->trackerOutParams.targetIndex;
        numClassifiedTargets = result->microDopplerOutParams.numClassifiedTargets;

        uDopplerData = (uint8_t*) result->microDopplerOutParams.uDopplerOutput;
        uDopplerFeatures = (uint8_t*) result->microDopplerOutParams.uDopplerFeatures;
        uClassifierOutput = (uint8_t*) &result->microDopplerOutParams.classifierOutput;

        numDopplerBins = result->microDopplerOutParams.numDopplerBins;

        packetLen = sizeof(MmwDemo_output_message_header);
        if ((pGuiMonSel->pointCloud == 1) && (result->numObjOut > 0))
        {
            tl[tlvIdx].type = MMWDEMO_OUTPUT_MSG_DETECTED_POINTS;
            tl[tlvIdx].length = sizeof(DPIF_PointCloudCartesian) * result->numObjOut;
            packetLen += sizeof(MmwDemo_output_message_tl) + tl[tlvIdx].length;
            tlvIdx++;
        }

        /* Side info */
        if ((pGuiMonSel->pointCloud == 1) && result->numObjOut > 0)
        {
            tl[tlvIdx].type = MMWDEMO_OUTPUT_MSG_DETECTED_POINTS_SIDE_INFO;
            tl[tlvIdx].length = sizeof(DPIF_PointCloudSideInfo) * result->numObjOut;
            packetLen += sizeof(MmwDemo_output_message_tl) + tl[tlvIdx].length;
            tlvIdx++;
        }

        /* Point Cloud Compressed format */
        if ((pGuiMonSel->pointCloud == 2) && (result->numObjOut > 0))
        {
            tl[tlvIdx].type = MMWDEMO_OUTPUT_EXT_MSG_DETECTED_POINTS;
            tl[tlvIdx].length = sizeof(MmwDemo_output_message_point_unit) +
                                sizeof(MmwDemo_output_message_UARTpoint) * result->numObjOut;
            packetLen += sizeof(MmwDemo_output_message_tl) + tl[tlvIdx].length;
            tlvIdx++;
        }

        /* Range Profile (Major motion) */
        if ((pGuiMonSel->rangeProfile & 0x1) && (gMmwMssMCB.rangeProfile[0] != NULL))
        {
            tl[tlvIdx].type = MMWDEMO_OUTPUT_EXT_MSG_RANGE_PROFILE_MAJOR;
            tl[tlvIdx].length = sizeof(uint32_t) * gMmwMssMCB.numRangeBins;
            packetLen += sizeof(MmwDemo_output_message_tl) + tl[tlvIdx].length;
            tlvIdx++;
        }
        /* Range Profile (Minor motion) */
        if ((pGuiMonSel->rangeProfile & 0x2) && (gMmwMssMCB.rangeProfile[1] != NULL))
        {
            tl[tlvIdx].type = MMWDEMO_OUTPUT_EXT_MSG_RANGE_PROFILE_MINOR;
            tl[tlvIdx].length = sizeof(uint32_t) * gMmwMssMCB.numRangeBins;
            packetLen += sizeof(MmwDemo_output_message_tl) + tl[tlvIdx].length;
            tlvIdx++;
        }

        /* Range-Azimuth Heatmap (Major motion) */
        if ((pGuiMonSel->rangeAzimuthHeatMap & 0x1) && (result->rngAzHeatMap[0] != NULL))
        {
            tl[tlvIdx].type = MMWDEMO_OUTPUT_EXT_MSG_RANGE_AZIMUT_HEAT_MAP_MAJOR;
            tl[tlvIdx].length = gMmwMssMCB.numRangeBins * gMmwMssMCB.sigProcChainCfg.azimuthFftSize * sizeof(uint32_t);
            packetLen += sizeof(MmwDemo_output_message_tl) +  tl[tlvIdx].length;
            tlvIdx++;
        }
        /* Range-Azimuth Heatmap (Minor motion) */
        if ((pGuiMonSel->rangeAzimuthHeatMap & 0x2) && (result->rngAzHeatMap[1] != NULL))
        {
            tl[tlvIdx].type = MMWDEMO_OUTPUT_EXT_MSG_RANGE_AZIMUT_HEAT_MAP_MINOR;
            tl[tlvIdx].length = gMmwMssMCB.numRangeBins * gMmwMssMCB.sigProcChainCfg.azimuthFftSize * sizeof(uint32_t);
            packetLen += sizeof(MmwDemo_output_message_tl) +  tl[tlvIdx].length;
            tlvIdx++;
        }

        if (pGuiMonSel->statsInfo)
        {
            memcpy((void*)gMmwMssMCB.outStats.tempReading, &tempStats.tempValue, (4 * sizeof(int16_t)));
            
            #ifdef INA228
            mmwDemo_PowerMeasurement(i2cHandle, &gMmwMssMCB.outStats.powerMeasured[0]);
            #else
            mmwDemo_PM_Null(i2cHandle, &gMmwMssMCB.outStats.powerMeasured[0]);
            #endif
            tl[tlvIdx].type = MMWDEMO_OUTPUT_EXT_MSG_STATS;
            tl[tlvIdx].length = sizeof(MmwDemo_output_message_stats);
            packetLen += sizeof(MmwDemo_output_message_tl) +  tl[tlvIdx].length;
            tlvIdx++;
        }

        if (pGuiMonSel->presenceInfo && mpdEnabled)
        {
            tl[tlvIdx].type = MMWDEMO_OUTPUT_EXT_MSG_ENHANCED_PRESENCE_INDICATION;
            tl[tlvIdx].length = ((uint8_t)ceil(gMmwMssMCB.sceneryParams.numBoundaryBoxes/4.0) + 1) * sizeof(uint8_t);
            packetLen += sizeof(MmwDemo_output_message_tl) + tl[tlvIdx].length;
            tlvIdx++;
        }

        if (pGuiMonSel->adcSamples)
        {
            tl[tlvIdx].type = MMWDEMO_OUTPUT_EXT_MSG_ADC_SAMPLES;
            tl[tlvIdx].length = gMmwMssMCB.numRxAntennas * gMmwMssMCB.numTxAntennas * gMmwMssMCB.profileComCfg.h_NumOfAdcSamples * sizeof(int16_t);
            packetLen += sizeof(MmwDemo_output_message_tl) +  tl[tlvIdx].length;
            tlvIdx++;
        }

        if (trackerEnabled && pGuiMonSel->trackerInfo)
        {
            if (numTargets > 0)
            {
                tl[tlvIdx].type = MMWDEMO_OUTPUT_EXT_MSG_TARGET_LIST;
                tl[tlvIdx].length = numTargets * sizeof(trackerProc_Target);
                packetLen += sizeof(MmwDemo_output_message_tl) + tl[tlvIdx].length;
                tlvIdx++;
            }
            if ((numIndices > 0) && (numTargets > 0))
            {
                tl[tlvIdx].type = MMWDEMO_OUTPUT_EXT_MSG_TARGET_INDEX;
                tl[tlvIdx].length = numIndices*sizeof(trackerProc_TargetIndex);
                packetLen += sizeof(MmwDemo_output_message_tl) + tl[tlvIdx].length;
                tlvIdx++;
            }
        }

        if (trackerEnabled && microDopplerEnabled)
        {
            if (pGuiMonSel->trackerInfo && pGuiMonSel->microDopplerInfo)
            {
                if (numTargets > 0)
                {
                    tl[tlvIdx].type = MMWDEMO_OUTPUT_EXT_MSG_MICRO_DOPPLER_RAW_DATA;
                    tl[tlvIdx].length = numTargets * numDopplerBins * sizeof(float);
                    packetLen += sizeof(MmwDemo_output_message_tl) + tl[tlvIdx].length;
                    tlvIdx++;

                    tl[tlvIdx].type = MMWDEMO_OUTPUT_EXT_MSG_MICRO_DOPPLER_FEATURES;
                    tl[tlvIdx].length = numTargets * sizeof(FeatExtract_featOutput);
                    packetLen += sizeof(MmwDemo_output_message_tl) + tl[tlvIdx].length;
                    tlvIdx++;
                }
            }
        }

        if (trackerEnabled && microDopplerEnabled && classifierEnabled)
        {
            if (pGuiMonSel->classifierInfo)
            {
                if (numClassifiedTargets > 0)
                {
                    tl[tlvIdx].type = MMWDEMO_OUTPUT_EXT_MSG_CLASSIFIER_INFO;
                    tl[tlvIdx].length = numClassifiedTargets * sizeof(DPU_uDopProc_classifierPrediction);
                    packetLen += sizeof(MmwDemo_output_message_tl) + tl[tlvIdx].length;
                    tlvIdx++;
                }
            }
        }

        if (gMmwMssMCB.measureRxChannelBiasCliCfg.enabled)
        {
            tl[tlvIdx].type = MMWDEMO_OUTPUT_EXT_MSG_RX_CHAN_COMPENSATION_INFO;
            tl[tlvIdx].length = sizeof(DPU_DoaProc_compRxChannelBiasFloatCfg);
            packetLen += sizeof(MmwDemo_output_message_tl) + tl[tlvIdx].length;
            tlvIdx++;
        }

        /* Fill header */
        header.numTLVs = tlvIdx;
        /* Round up packet length to multiple of MMWDEMO_OUTPUT_MSG_SEGMENT_LEN */
        header.totalPacketLen = MMWDEMO_OUTPUT_MSG_SEGMENT_LEN *
                ((packetLen + (MMWDEMO_OUTPUT_MSG_SEGMENT_LEN-1))/MMWDEMO_OUTPUT_MSG_SEGMENT_LEN);
        header.timeCpuCycles =  0; //TODO: Populate with actual time
        header.frameNumber = gMmwMssMCB.stats.frameStartIntCounter; //result->stats->frameStartIntCounter; TODO: Check this
        header.subFrameNumber = -1;

        if(tlvIdx != 0)
        {
            mmw_UartWrite (uartHandle, (uint8_t*)&header, sizeof(MmwDemo_output_message_header));
            tlvIdx = 0;
        }

        /* Send detected Objects */
        if ((pGuiMonSel->pointCloud == 1) && (result->numObjOut > 0))
        {
            mmw_UartWrite (uartHandle,
                            (uint8_t*)&tl[tlvIdx],
                            sizeof(MmwDemo_output_message_tl));

            /*Send array of objects */
            mmw_UartWrite (uartHandle, (uint8_t*)result->objOut,
                            sizeof(DPIF_PointCloudCartesian) * result->numObjOut);
            tlvIdx++;
        }

        /* Send detected Objects Side Info */
        if ((pGuiMonSel->pointCloud == 1) && (result->numObjOut > 0))
        {
            mmw_UartWrite (uartHandle,
                            (uint8_t*)&tl[tlvIdx],
                            sizeof(MmwDemo_output_message_tl));

            /*Send array of objects */
            mmw_UartWrite (uartHandle, (uint8_t*)result->objOutSideInfo,
                            sizeof(DPIF_PointCloudSideInfo) * result->numObjOut);
            tlvIdx++;
        }

        /* Send Point Cloud Compressed format */
        if ((pGuiMonSel->pointCloud == 2) && (result->numObjOut > 0))
        {
            /*Send point cloud */
            gMmwMssMCB.pointCloudToUart.header = tl[tlvIdx];
            mmw_UartWrite (uartHandle, (uint8_t*)&gMmwMssMCB.pointCloudToUart,
                           sizeof(MmwDemo_output_message_tl) + tl[tlvIdx].length);
            tlvIdx++;
        }

        /* Send Range profile (Major mode) */
        if ((pGuiMonSel->rangeProfile & 0x1) && (gMmwMssMCB.rangeProfile[0] != NULL))
        {
            mmw_UartWrite (uartHandle,
                            (uint8_t*)&tl[tlvIdx],
                            sizeof(MmwDemo_output_message_tl));

            mmw_UartWrite (uartHandle,
                            (uint8_t*)gMmwMssMCB.rangeProfile[0],
                            (sizeof(uint32_t)*(mathUtils_pow2roundup(gMmwMssMCB.profileComCfg.h_NumOfAdcSamples)/2)));
            tlvIdx++;
        }
        /* Send Range profile (Minor mode) */
        if ((pGuiMonSel->rangeProfile & 0x2) && (gMmwMssMCB.rangeProfile[1] != NULL))
        {
            mmw_UartWrite (uartHandle,
                            (uint8_t*)&tl[tlvIdx],
                            sizeof(MmwDemo_output_message_tl));

            mmw_UartWrite (uartHandle,
                            (uint8_t*)gMmwMssMCB.rangeProfile[1],
                            (sizeof(uint32_t)*(mathUtils_pow2roundup(gMmwMssMCB.profileComCfg.h_NumOfAdcSamples)/2)));
            tlvIdx++;
        }

        /* Send Range-Azimuth Heatmap (Major motion) */
        if ((pGuiMonSel->rangeAzimuthHeatMap & 0x1) && (result->rngAzHeatMap[0] != NULL))
        {
            mmw_UartWrite (uartHandle,
                            (uint8_t*)&tl[tlvIdx],
                            sizeof(MmwDemo_output_message_tl));

            mmw_UartWrite (uartHandle,
                    (uint8_t *) result->rngAzHeatMap[0],
                    (gMmwMssMCB.numRangeBins * gMmwMssMCB.sigProcChainCfg.azimuthFftSize * sizeof(uint32_t)));

            tlvIdx++;
        }
        /* Send Range-Azimuth Heatmap (Minor motion) */
        if ((pGuiMonSel->rangeAzimuthHeatMap & 0x2) && (result->rngAzHeatMap[1] != NULL))
        {
            mmw_UartWrite (uartHandle,
                            (uint8_t*)&tl[tlvIdx],
                            sizeof(MmwDemo_output_message_tl));

            mmw_UartWrite (uartHandle,
                    (uint8_t *) result->rngAzHeatMap[1],
                    (gMmwMssMCB.numRangeBins * gMmwMssMCB.sigProcChainCfg.azimuthFftSize * sizeof(uint32_t)));

            tlvIdx++;
        }

        /* Send stats information (interframe processing time and uart transfer time) */
        if (pGuiMonSel->statsInfo)
        {
            mmw_UartWrite (uartHandle,
                            (uint8_t*)&tl[tlvIdx],
                            sizeof(MmwDemo_output_message_tl));

            mmw_UartWrite (uartHandle,
                           (uint8_t*) &gMmwMssMCB.outStats,
                           tl[tlvIdx].length);
            tlvIdx++;
        }

        /* Send motion, presence detected for each zone (0th index - No. of zones processed, 1st index onwards - 2bits state per zone from LSB) */
        if (pGuiMonSel->presenceInfo && mpdEnabled)
        {
            mmw_UartWrite (uartHandle,
                            (uint8_t*)&tl[tlvIdx],
                            sizeof(MmwDemo_output_message_tl));

            mmw_UartWrite (uartHandle,
                           (uint8_t*) gMmwMssMCB.dpcZoneState,
                           tl[tlvIdx].length);
            tlvIdx++;
        }
        /* Send ADC samples of last chirp pair in the frame */
        if (pGuiMonSel->adcSamples)
        {
            CSL_app_hwa_adcbuf_ctrlRegs *ptrAdcBufCtrlRegs = (CSL_app_hwa_adcbuf_ctrlRegs *)CSL_APP_HWA_ADCBUF_CTRL_U_BASE;

            mmw_UartWrite (uartHandle,
                            (uint8_t*)&tl[tlvIdx],
                            sizeof(MmwDemo_output_message_tl));

            if (gMmwMssMCB.numTxAntennas == 2)
            {
                /* Set view to ADC ping buffer */
                CSL_FINS(ptrAdcBufCtrlRegs->ADCBUFCFG1, APP_HWA_ADCBUF_CTRL_ADCBUFCFG1_ADCBUFCFG1_ADCBUFPIPOOVRCNT, 1);

                mmw_UartWrite (uartHandle,
                               (uint8_t*) CSL_APP_HWA_ADCBUF_RD_U_BASE,
                               tl[tlvIdx].length/2);

                /* Set view to ADC pong buffer */
                CSL_FINS(ptrAdcBufCtrlRegs->ADCBUFCFG1, APP_HWA_ADCBUF_CTRL_ADCBUFCFG1_ADCBUFCFG1_ADCBUFPIPOOVRCNT, 0);

                mmw_UartWrite (uartHandle,
                               (uint8_t*) CSL_APP_HWA_ADCBUF_RD_U_BASE,
                               tl[tlvIdx].length/2);
            }
            else
            {
                /* Only one Tx antenna from ping buffer */
                /* Set view to ADC ping buffer */
                CSL_FINS(ptrAdcBufCtrlRegs->ADCBUFCFG1, APP_HWA_ADCBUF_CTRL_ADCBUFCFG1_ADCBUFCFG1_ADCBUFPIPOOVRCNT, 1);

                mmw_UartWrite (uartHandle,
                               (uint8_t*) CSL_APP_HWA_ADCBUF_RD_U_BASE,
                               tl[tlvIdx].length);

                /* Set view to ADC pong buffer */
                CSL_FINS(ptrAdcBufCtrlRegs->ADCBUFCFG1, APP_HWA_ADCBUF_CTRL_ADCBUFCFG1_ADCBUFCFG1_ADCBUFPIPOOVRCNT, 0);

            }
            tlvIdx++;

        }

        /* Send Group Tracker output */
        if (trackerEnabled && pGuiMonSel->trackerInfo)
        {
            if (numTargets > 0)
            {
                mmw_UartWrite (uartHandle,
                                   (uint8_t*)&tl[tlvIdx],
                                   sizeof(MmwDemo_output_message_tl));
                mmw_UartWrite (uartHandle,
                                   (uint8_t*)tList,
                                   tl[tlvIdx].length);
                tlvIdx++;
            }
            if ((numIndices > 0) && (numTargets > 0))
            {
                mmw_UartWrite (uartHandle,
                                   (uint8_t*)&tl[tlvIdx],
                                   sizeof(MmwDemo_output_message_tl));
                mmw_UartWrite (uartHandle,
                                   (uint8_t*)tIndex,
                                   tl[tlvIdx].length);
                tlvIdx++;
            }
        }

        /* Send microDoppler output */
        if (trackerEnabled && microDopplerEnabled)
        {
            if (pGuiMonSel->trackerInfo && pGuiMonSel->microDopplerInfo)
            {
                if (numTargets > 0)
                {
                    /* Micro Doppler raw data */
                    mmw_UartWrite (uartHandle,
                                       (uint8_t*)&tl[tlvIdx],
                                       sizeof(MmwDemo_output_message_tl));
                    mmw_UartWrite (uartHandle,
                                       (uint8_t*)uDopplerData,
                                       tl[tlvIdx].length);
                    tlvIdx++;

                    /* Micro Doppler features */
                    mmw_UartWrite (uartHandle,
                                       (uint8_t*)&tl[tlvIdx],
                                       sizeof(MmwDemo_output_message_tl));
                    mmw_UartWrite (uartHandle,
                                       (uint8_t*)uDopplerFeatures,
                                       tl[tlvIdx].length);
                    tlvIdx++;
                }
            }
        }

        /* Send Classifier output - predictions */
        if (trackerEnabled && microDopplerEnabled && classifierEnabled)
        {
            if (pGuiMonSel->classifierInfo)
            {
                if (numClassifiedTargets > 0)
                {
                    /* Classifier output - predictions */
                    mmw_UartWrite (uartHandle,
                                       (uint8_t*)&tl[tlvIdx],
                                       sizeof(MmwDemo_output_message_tl));
                    mmw_UartWrite (uartHandle,
                                       (uint8_t*)uClassifierOutput,
                                       tl[tlvIdx].length);
                    tlvIdx++;
                }
            }
        }

        /* Send Rx Channel compensation coefficients */
        if (gMmwMssMCB.measureRxChannelBiasCliCfg.enabled)
        {
            mmw_UartWrite (uartHandle,
                               (uint8_t*)&tl[tlvIdx],
                               sizeof(MmwDemo_output_message_tl));
            mmw_UartWrite (uartHandle,
                               (uint8_t*)&gMmwMssMCB.compRxChannelBiasCfgMeasureOut,
                               tl[tlvIdx].length);
            tlvIdx++;
        }

        if(tlvIdx != 0)
        {
            /* Send padding bytes */
            numPaddingBytes = MMWDEMO_OUTPUT_MSG_SEGMENT_LEN - (packetLen & (MMWDEMO_OUTPUT_MSG_SEGMENT_LEN-1));
            if (numPaddingBytes < MMWDEMO_OUTPUT_MSG_SEGMENT_LEN)
            {
                mmw_UartWrite (uartHandle, (uint8_t*)padding, numPaddingBytes);
            }
        }
        /* Flush UART buffer here for each frame. */
        UART_flushTxFifo(uartHandle);

        /* End of UART data transmission */
        gMmwMssMCB.stats.uartTransferEndTimeStamp = Cycleprofiler_getTimeStamp();
        gMmwMssMCB.outStats.transmitOutputTime = (gMmwMssMCB.stats.uartTransferEndTimeStamp - gMmwMssMCB.stats.interFrameEndTimeStamp)/FRAME_REF_TIMER_CLOCK_MHZ;
        gMmwMssMCB.stats.totalActiveTime_us = (gMmwMssMCB.stats.uartTransferEndTimeStamp - gMmwMssMCB.stats.frameStartTimeStamp)/FRAME_REF_TIMER_CLOCK_MHZ;

        //Interframe processing and UART data transmission completed ToDo check if we want to put in execute task, since UART transmission can prolong till the chirping finishes
        gMmwMssMCB.interSubFrameProcToken--;

        // Capture the time elaspsed till here
        demoEndTime = PRCMSlowClkCtrGet();
        // Demo Run time for one frame (From Sensor Start till Completion of UART transmit)
        demoTimeus = (demoEndTime - demoStartTime)*(30.5);
        if (gMmwMssMCB.adcDataSourceCfg.source == 1 || gMmwMssMCB.adcDataSourceCfg.source == 2)
        {
            demoTimeus = 0;
        }
        
        if((gMmwMssMCB.lowPowerMode == LOW_PWR_MODE_ENABLE) || (gMmwMssMCB.lowPowerMode == LOW_PWR_TEST_MODE))
        {
            xSemaphoreGive(gPowerSem);
            if(gMmwMssMCB.lowPowerMode == LOW_PWR_MODE_ENABLE)
            {
                Power_enablePolicy();
            }
        }

        if (gMmwMssMCB.lowPowerMode == LOW_PWR_MODE_DISABLE)
        {
            if(gMmwMssMCB.adcDataSourceCfg.source == 1 || gMmwMssMCB.adcDataSourceCfg.source == 2)
            {
                /* In test mode trigger next frame processing */
                SemaphoreP_post(&gMmwMssMCB.adcFileTaskSemHandle);
            }
            // Imp Note: Sensor Stop command is honoured only when Low Power Cfg is disabled
            if(sensorStop == 1)
            {
                int32_t err;
                // Stop and Close the front end
                MMWave_stop(gMmwMssMCB.ctrlHandle,&err);
                MMWave_close(gMmwMssMCB.ctrlHandle,&err);
                // Delete the exisitng profile as we receive a new configuration
                MMWave_delProfile(gMmwMssMCB.ctrlHandle,gMmwMssMCB.mmwCtrlCfg.frameCfg[0].profileHandle[0],&err);
                // Free up all the edma channels and close the EDMA interface 
                mmwDemo_freeDmaChannels(gEdmaHandle[0]);
                Drivers_edmaClose();
                EDMA_deinit();
                // Demo Stopped
                rangeProcHWAObj* temp = gMmwMssMCB.rangeProcDpuHandle;
                temp->inProgress = false;
                gMmwMssMCB.oneTimeConfigDone = 0;
                // Re-init the EDMA interface for next configuration
                EDMA_init();
                Drivers_edmaOpen();
                gMmwMssMCB.stats.frameStartIntCounter = 0;
                sensorStop = 0;
                isSensorStarted = 0;
            
                // Delete the DPC, TLV as we will create them again in next configuration
                vTaskDelete(gDpcTask);
                vTaskDelete(NULL);
            }
        }
    }
}

/**
 *  @b Description
 *  @n  DPC processing chain execute function.
 *
 */
void mmwDemo_DpcExecute(){
    int32_t retVal;
    int32_t errCode = 0;
    int32_t i;
    DPU_RangeProcHWA_OutParams outParms;
    DPU_DoaProc_OutParams outParmsDoaproc;
    DPU_CFARProcHWA_OutParams outParmsCfar;
    DPU_MpdProc_OutParams outParmsMpdproc;
    uint8_t enableMajorMotion;
    uint8_t enableMinorMotion;
    int16_t skipFrmCntr = 0;//gMmwMssMCB.sigProcChainCfg.numFrmPerMinorMotProc - 1; //We start minor motion from the first frame although the radar cube is not fully filled 
    DPC_ObjectDetection_ExecuteResult *result = &gMmwMssMCB.dpcResult;
    uint32_t numDetectedPoints[2]; //numDetectedPoints[0] - Number of points in major motion detection, numDetectedPoints[0] - Number of points in minor motion detection,

    DPU_uDopProc_TrackerData trackerData;

    /* give initial trigger for the first frame */
    errCode = DPU_RangeProcHWA_control(gMmwMssMCB.rangeProcDpuHandle,
                 DPU_RangeProcHWA_Cmd_triggerProc, NULL, 0);
    if(errCode < 0)
    {
        CLI_write("Error: Range control execution failed [Error code %d]\n", errCode);
    }

    if (gMmwMssMCB.sigProcChainCfg.motDetMode == 1)
    {
        enableMajorMotion = 1;
        enableMinorMotion = 0;
    }
    else if (gMmwMssMCB.sigProcChainCfg.motDetMode == 3)
    {
        enableMajorMotion = 1;
        enableMinorMotion = 1;
    }
    else
    {
        enableMajorMotion = 0;
        enableMinorMotion = 1;
    }
    if (enableMajorMotion)
    {
        result->rngAzHeatMap[MMW_DEMO_MAJOR_MODE] = (uint32_t *) gMmwMssMCB.detMatrix[MMW_DEMO_MAJOR_MODE].data;
    }
    else
    {
        result->rngAzHeatMap[MMW_DEMO_MAJOR_MODE] = NULL;
    }
    if (enableMinorMotion)
    {
        result->rngAzHeatMap[MMW_DEMO_MINOR_MODE] = (uint32_t *) gMmwMssMCB.detMatrix[MMW_DEMO_MINOR_MODE].data;
    }
    else
    {
        result->rngAzHeatMap[MMW_DEMO_MINOR_MODE] = NULL;
    }

    result->objOut = gMmwMssMCB.dpcObjOut;
    result->objOutSideInfo = gMmwMssMCB.dpcObjSideInfo;

    while(true){

        memset((void *)&outParms, 0, sizeof(DPU_RangeProcHWA_OutParams));
        retVal = DPU_RangeProcHWA_process(gMmwMssMCB.rangeProcDpuHandle, &outParms);
        if(retVal != 0){
            CLI_write("DPU_RangeProcHWA_process failed with error code %d", retVal);
            DebugP_assert(0);
        }

        /* Procedure for range bias measurement and Rx channels gain/phase offset measurement */
        if(gMmwMssMCB.measureRxChannelBiasCliCfg.enabled)
        {
            mmwDemo_rangeBiasRxChPhaseMeasure();
        }


        // Read the temperature
        MMWave_getTemperatureReport(&tempStats);

        if((gMmwMssMCB.lowPowerMode == LOW_PWR_MODE_ENABLE) || (gMmwMssMCB.lowPowerMode == LOW_PWR_TEST_MODE))
        {
            //Shutdown the FECSS after chirping
            // Retain FECSS Code Memory
            int32_t err;
            PRCMSetSRAMRetention((PRCM_FEC_PD_SRAM_CLUSTER_2 | PRCM_FEC_PD_SRAM_CLUSTER_3), PRCM_SRAM_LPDS_RET);

            //Reset The FrameTimer for next frame
            HW_WR_REG32(CSL_APP_RCM_U_BASE + CSL_APP_RCM_BLOCKRESET2, 0x1c0);
            for(int i =0;i<10;i++)
            {
                test = PRCMSlowClkCtrGet();
            }
            HW_WR_REG32(CSL_APP_RCM_U_BASE + CSL_APP_RCM_BLOCKRESET2, 0x0);

            // MMW Closure in preparation for Low power state
            MMWave_stop(gMmwMssMCB.ctrlHandle,&err);
            MMWave_close(gMmwMssMCB.ctrlHandle,&err);
            MMWave_deinit(gMmwMssMCB.ctrlHandle,&err);
        }

        /* Chirping finished start interframe processing */
        gMmwMssMCB.stats.interFrameStartTimeStamp = Cycleprofiler_getTimeStamp();
        gMmwMssMCB.stats.chirpingTime_us = (gMmwMssMCB.stats.interFrameStartTimeStamp - gMmwMssMCB.stats.frameStartTimeStamp)/FRAME_REF_TIMER_CLOCK_MHZ;

        /* Trigger doaproc, CFAR and mpdproc DPUs subsequently */
        memset((void *)&outParmsDoaproc, 0, sizeof(DPU_DoaProc_OutParams));
        memset((void *)&outParmsCfar, 0, sizeof(DPU_CFARProcHWA_OutParams));
        memset((void *)&outParmsMpdproc, 0, sizeof(DPU_MpdProc_OutParams));

        numDetectedPoints[MMW_DEMO_MAJOR_MODE] = 0;
        if (enableMajorMotion)
        {
            memset((void *)&outParmsDoaproc, 0, sizeof(DPU_DoaProc_OutParams));
            retVal = DPU_DoaProc_process(gMmwMssMCB.doaProcDpuHandle, &gMmwMssMCB.radarCubeSrc[MMW_DEMO_MAJOR_MODE], &gMmwMssMCB.detMatrix[MMW_DEMO_MAJOR_MODE], &outParmsDoaproc);
            if(retVal != 0){
                CLI_write("DPU_DoaProc_process failed with error code %d", retVal);
                DebugP_assert(0);
            }

            memset((void *)&outParmsCfar, 0, sizeof(DPU_CFARProcHWA_OutParams));
            outParmsCfar.rangeProfile = gMmwMssMCB.rangeProfile[MMW_DEMO_MAJOR_MODE];
            outParmsCfar.detObjOut = &gMmwMssMCB.cfarDetObjOut[0];
            outParmsCfar.detObjIndOut = &gMmwMssMCB.dpcObjIndOut[0];
            outParmsCfar.detObjOutMaxSize = MAX_NUM_DETECTIONS;
            retVal = DPU_CFARProcHWA_process(gMmwMssMCB.cfarProcDpuHandle,
                                             &gMmwMssMCB.detMatrix[MMW_DEMO_MAJOR_MODE],
                                             &gMmwMssMCB.cfarRunTimeInputParams[MMW_DEMO_MAJOR_MODE],
                                             &outParmsCfar);
            numDetectedPoints[MMW_DEMO_MAJOR_MODE] = outParmsCfar.numCfarDetectedPoints;


            if(retVal != 0){
                CLI_write("DPU_CFARProcHWA_process failed with error code %d", retVal);
                DebugP_assert(0);
            }
        }

        numDetectedPoints[MMW_DEMO_MINOR_MODE] = 0;
        if (enableMinorMotion)
        {
            if (skipFrmCntr > 0)
            {
                skipFrmCntr--;
            }
            else
            {
                memset((void *)&outParmsDoaproc, 0, sizeof(DPU_DoaProc_OutParams));

                retVal = DPU_DoaProc_process(gMmwMssMCB.doaProcDpuHandle, &gMmwMssMCB.radarCubeSrc[MMW_DEMO_MINOR_MODE], &gMmwMssMCB.detMatrix[MMW_DEMO_MINOR_MODE], &outParmsDoaproc);
                if(retVal != 0){
                    CLI_write("DPU_DoaProc_process failed with error code %d", retVal);
                    DebugP_assert(0);
                }

                memset((void *)&outParmsCfar, 0, sizeof(DPU_CFARProcHWA_OutParams));
                outParmsCfar.rangeProfile = gMmwMssMCB.rangeProfile[MMW_DEMO_MINOR_MODE];
                outParmsCfar.detObjOut = &gMmwMssMCB.cfarDetObjOut[numDetectedPoints[MMW_DEMO_MAJOR_MODE]];
                outParmsCfar.detObjIndOut = &gMmwMssMCB.dpcObjIndOut[numDetectedPoints[MMW_DEMO_MAJOR_MODE]];
                outParmsCfar.detObjOutMaxSize = MAX_NUM_DETECTIONS - numDetectedPoints[MMW_DEMO_MAJOR_MODE];
                retVal = DPU_CFARProcHWA_process(gMmwMssMCB.cfarProcDpuHandle,
                                                 &gMmwMssMCB.detMatrix[MMW_DEMO_MINOR_MODE],
                                                 &gMmwMssMCB.cfarRunTimeInputParams[MMW_DEMO_MINOR_MODE],
                                                 &outParmsCfar);
                numDetectedPoints[MMW_DEMO_MINOR_MODE] = outParmsCfar.numCfarDetectedPoints;

                if(retVal != 0){
                    CLI_write("DPU_CFARProcHWA_process failed with error code %d", retVal);
                    DebugP_assert(0);
                }
            }
        }

        result->numObjOutMajor = numDetectedPoints[MMW_DEMO_MAJOR_MODE];
        result->numObjOutMinor = numDetectedPoints[MMW_DEMO_MINOR_MODE];
        result->numObjOut = numDetectedPoints[MMW_DEMO_MAJOR_MODE] + numDetectedPoints[MMW_DEMO_MINOR_MODE];

        if (gMmwMssMCB.isMotionPresenceDpuEnabled)
        {
            outParmsMpdproc.mpdZoneState = gMmwMssMCB.dpcZoneState;

            retVal = DPU_MpdProc_process(gMmwMssMCB.mpdProcDpuHandle, &outParmsMpdproc);
            if(retVal != 0){
                CLI_write("DPU_MpdProc_process failed with error code %d", retVal);
                DebugP_assert(0);
            }
        }
        if(gMmwMssMCB.guiMonSel.pointCloud == 1)
        {
            for(i=0; i < result->numObjOut; i++)
            {
                result->objOut[i].x = gMmwMssMCB.cfarDetObjOut[i].x;
                result->objOut[i].y = gMmwMssMCB.cfarDetObjOut[i].y;
                result->objOut[i].z = gMmwMssMCB.cfarDetObjOut[i].z;
                result->objOut[i].velocity = gMmwMssMCB.cfarDetObjOut[i].velocity;
                result->objOutSideInfo[i].snr = (int16_t) (10. * gMmwMssMCB.cfarDetObjOut[i].snr); //steps of 0.1dB
                result->objOutSideInfo[i].noise = (int16_t) (10. * gMmwMssMCB.cfarDetObjOut[i].noise); //steps of 0.1dB
            }
        }

        if(gMmwMssMCB.guiMonSel.pointCloud == 2)
        {
            /* Compress point cloud list (data converted from floating point to fix point) */
            MmwDemo_compressPointCloudList(&gMmwMssMCB.pointCloudToUart,
                                           &gMmwMssMCB.pointCloudUintRecip,
                                           gMmwMssMCB.cfarDetObjOut,
                                           result->numObjOut);
            gMmwMssMCB.pointCloudToUart.pointUint.numDetectedPoints[0] = numDetectedPoints[0]; //Number of points in major motion mode
            gMmwMssMCB.pointCloudToUart.pointUint.numDetectedPoints[1] = numDetectedPoints[1]; //Number of points in minor motion mode
        }


        if(gMmwMssMCB.trackerCfg.staticCfg.trackerEnabled)
        {
            uint32_t trackerStartTime = Cycleprofiler_getTimeStamp();

            /* Group tracker DPU */
            retVal = DPU_TrackerProc_process(gMmwMssMCB.trackerProcDpuHandle,
                                            (uint16_t) (numDetectedPoints[0] + numDetectedPoints[1]),
                                            gMmwMssMCB.cfarDetObjOut,
                                            &result->trackerOutParams);
            if (retVal != 0)
            {
                CLI_write("Error: DPU_TrackerProc_process failed with error code %d", retVal);
                DebugP_assert(0);
            }
            gMmwMssMCB.stats.trackerTime_us = (Cycleprofiler_getTimeStamp() - trackerStartTime)/FRAME_REF_TIMER_CLOCK_MHZ;
        }

        if (gMmwMssMCB.microDopplerCliCfg.enabled)
        {
            uint32_t microDopplerStartTime = Cycleprofiler_getTimeStamp();
            trackerData.numTargets = result->trackerOutParams.numTargets;
            trackerData.numIndices = result->trackerOutParams.numIndices;
            trackerData.numIndicesMajorMotion = numDetectedPoints[0];
            trackerData.numIndicesMinorMotion = numDetectedPoints[1];
            trackerData.tIndex = result->trackerOutParams.targetIndex;

            for (i = 0; i < trackerData.numTargets; i++)
            {
                trackerData.tList[i].tid  = result->trackerOutParams.tList[i].tid;
                trackerData.tList[i].posX = result->trackerOutParams.tList[i].posX;
                trackerData.tList[i].posY = result->trackerOutParams.tList[i].posY;
                trackerData.tList[i].posZ = result->trackerOutParams.tList[i].posZ;
                if (gMmwMssMCB.microDopplerCliCfg.circShiftAroundCentroid)
                {
                    trackerData.tList[i].velX = result->trackerOutParams.tList[i].velX;
                    trackerData.tList[i].velY = result->trackerOutParams.tList[i].velY;
                    trackerData.tList[i].velZ = result->trackerOutParams.tList[i].velZ;
                }
            }

            result->microDopplerOutParams.uDopplerOutput = gMmwMssMCB.uDopProcOutParams.uDopplerOutput;
            result->microDopplerOutParams.uDopplerFeatures = gMmwMssMCB.uDopProcOutParams.uDopplerFeatures;

            retVal = DPU_uDopProc_process(gMmwMssMCB.microDopDpuHandle,
                                         &gMmwMssMCB.radarCube[0], //Major motion radar cube
                                         &gMmwMssMCB.detMatrix[0], //Major motion detection matrix
                                         &trackerData,
                                         &result->microDopplerOutParams);
            if (retVal != 0)
            {
                CLI_write("Error: DPU_uDopProc_process failed with error code %d", retVal);
                DebugP_assert(0);
            }
            gMmwMssMCB.stats.microDopplerDpuTime_us = (Cycleprofiler_getTimeStamp() - microDopplerStartTime)/FRAME_REF_TIMER_CLOCK_MHZ;
            gMmwMssMCB.stats.featureExtractionTime_us = result->microDopplerOutParams.stats.featureExtractTime_cpuCycles/FRAME_REF_TIMER_CLOCK_MHZ;
            gMmwMssMCB.stats.classifierTime_us = result->microDopplerOutParams.stats.classifierTime_cpuCycles/FRAME_REF_TIMER_CLOCK_MHZ;

        }

        if (gMmwMssMCB.adcDataSourceCfg.source == 2)
        {
            ClockP_sleep(1);
        }
        
        /* Give initial trigger for the next frame */
        retVal = DPU_RangeProcHWA_control(gMmwMssMCB.rangeProcDpuHandle,
                    DPU_RangeProcHWA_Cmd_triggerProc, NULL, 0);
        if(retVal < 0)
        {
            CLI_write("Error: DPU_RangeProcHWA_control failed with error code %d", retVal);
            DebugP_assert(0);
        }


        /* Interframe processing finished */
        gMmwMssMCB.stats.interFrameEndTimeStamp = Cycleprofiler_getTimeStamp();
        gMmwMssMCB.outStats.interFrameProcessingTime = (gMmwMssMCB.stats.interFrameEndTimeStamp - gMmwMssMCB.stats.interFrameStartTimeStamp)/FRAME_REF_TIMER_CLOCK_MHZ;

        /* Trigger UART task to send TLVs to host */
        SemaphoreP_post(&gMmwMssMCB.tlvSemHandle);
    }
}

/**
*  @b Description
*  @n
*        Function configuring and executing DPC
*/
void mmwDemo_dpcTask()
{
    
    mmwDemo_DpcConfig();

    mmwDemo_DpcExecute();

    /* Never return for this task. */
    SemaphoreP_pend(&gMmwMssMCB.TestSemHandle, SystemP_WAIT_FOREVER);
}

int16_t *gPreStoredAdcTestBuff;
int32_t gPreStoredAdcTestBuffInd = 0;
int32_t gPreStoredAdcTestBuffRdInd = 0;

uint32_t  localRead(uint8_t * adcTestBuff, uint32_t sizeOfSamp,  uint32_t numSamp)
{
    memcpy((uint8_t *)adcTestBuff, (uint8_t *)&gPreStoredAdcTestBuff[gPreStoredAdcTestBuffInd], sizeOfSamp * numSamp);
    gPreStoredAdcTestBuffInd += numSamp;
    return numSamp;
}

#if defined(LOW_POWER_DEEP_SLEEP_MODE_VERIFICATION)

uint32_t *gStoredHeatMap;
uint32_t gStoredHeatMapInd = 0;

void localWrite(uint32_t *detMAtrixData, uint32_t sizeOfSamp, uint32_t numSamp, FILE *fileIdDetMatData)
{
    memcpy((uint8_t *)&gStoredHeatMap[gStoredHeatMapInd], (uint8_t *) detMAtrixData, sizeOfSamp * numSamp);
    gStoredHeatMapInd += numSamp;
}
#endif

/**
*  @b Description
*  @n
*        Function to read ADC data from file. For testing purpose only.
*/
void mmwDemo_adcFileReadTask(){

    uint32_t baseAddr, regionId, numAdcSamplesPerEvt, numReadSamples;
    int32_t  errorCode = 0;
    int32_t status = SystemP_SUCCESS;
    uint16_t frameCnt, i, j;
    bool endOfFile = false;
    FILE * fileIdAdcData;
    FILE * fileIdDetMatData;

    //FILE * fileIdDetMatDataRef;  //ToDo Remove this

    FILE * fileIdPointCloudIndData;
    rangeProcTestConfig_t testConfig;
    char fileOutName[DPC_ADC_FILENAME_MAX_LEN];
    char *ptr;
    uint32_t *detMAtrixData;
#ifndef LOW_POWER_DEEP_SLEEP_MODE_VERIFICATION
    DPC_ObjectDetection_ExecuteResult *result = &gMmwMssMCB.dpcResult;
#endif
    if (gMmwMssMCB.enableMajorMotion)
    {
        detMAtrixData = (uint32_t *) gMmwMssMCB.detMatrix[0].data;
    }
    else
    {
        detMAtrixData = (uint32_t *) gMmwMssMCB.detMatrix[1].data;
    }

    baseAddr = EDMA_getBaseAddr(gEdmaHandle[0]);
    DebugP_assert(baseAddr != 0);

    regionId = EDMA_getRegionId(gEdmaHandle[0]);
    DebugP_assert(regionId < SOC_EDMA_NUM_REGIONS);

    /* start the test */
    if (gMmwMssMCB.adcDataSourceCfg.source == 1)
    {
        fileIdAdcData = fopen(gMmwMssMCB.adcDataSourceCfg.fileName, "rb");
        if (fileIdAdcData == NULL)
        {
            printf("Error:  Cannot open ADC file !\n");
            exit(0);
        }

#if 0
//ToDo: Remove this below, it just for verifying CFAR:
    /* Open output file for detection matrix target */
    strcpy(fileOutName, gMmwMssMCB.adcDataSourceCfg.fileName);
    ptr = strrchr(fileOutName, '/');
    if (ptr == NULL)
    {
        strcpy(fileOutName, "detMatRef.bin");
    }
    else
    {
        strcpy(&ptr[1], "detMatRef.bin");
    }
    fileIdDetMatDataRef = fopen(fileOutName, "rb");
//ToDo: Remove this above, it just for verifying CFAR:
#endif


        /* Open output file for detection matrix target */
        strcpy(fileOutName, gMmwMssMCB.adcDataSourceCfg.fileName);
        ptr = strrchr(fileOutName, '/');
        if (ptr == NULL)
        {
            strcpy(fileOutName, "detMatTarget.bin");
        }
        else
        {
            strcpy(&ptr[1], "detMatTarget.bin");
        }
        fileIdDetMatData = fopen(fileOutName, "wb");

        /* Open output file for point cloud list: range/azimuth/elevation/Doppler indices  */
        strcpy(fileOutName, gMmwMssMCB.adcDataSourceCfg.fileName);
        ptr = strrchr(fileOutName, '/');
        if (ptr == NULL)
        {
            strcpy(fileOutName, "pcloudIndTarget.bin");
        }
        else
        {
            strcpy(&ptr[1], "pcloudIndTarget.bin");
        }
        fileIdPointCloudIndData = fopen(fileOutName, "wb");
    }


    /* read in test config */
    testConfig.numRxAntennas = gMmwMssMCB.numRxAntennas;

    if (gMmwMssMCB.adcDataSourceCfg.source == 1)
    {
    fread(&testConfig.numAdcSamples, sizeof(uint32_t),1,fileIdAdcData);
    fread(&testConfig.numVirtualAntennas, sizeof(uint32_t),1,fileIdAdcData);
    fread(&testConfig.numChirpsPerFrame, sizeof(uint32_t),1,fileIdAdcData);
    fread(&testConfig.numFrames, sizeof(uint32_t),1,fileIdAdcData);
    }

    else
    {
        //CLI_write ("\n\nInput config data from file\n\n");

        //CLI_write ("No of adc samples:/>");
        status = CLI_readLine(gUartHandle[0], (char*)&testConfig.numAdcSamples, READ_LINE_BUFSIZE);
        if(status != SystemP_SUCCESS)
        {
            CLI_write("Error reading input config\n");
            DebugP_assert(0);
        }
        testConfig.numAdcSamples = atoi ((char*)&testConfig.numAdcSamples);
        
        //CLI_write ("\nNo of virtual antennas:/>");
        status = CLI_readLine(gUartHandle[0], (char*)&testConfig.numVirtualAntennas, READ_LINE_BUFSIZE);
        if(status != SystemP_SUCCESS)
        {
            CLI_write("Error reading input config\n");
            DebugP_assert(0);
        }
        testConfig.numVirtualAntennas = atoi ((char*)&testConfig.numVirtualAntennas);
        
        //CLI_write ("\nNo of chirps per frame:/>");
        status = CLI_readLine(gUartHandle[0], (char*)&testConfig.numChirpsPerFrame, READ_LINE_BUFSIZE);
        if(status != SystemP_SUCCESS)
        {
            CLI_write("Error reading input config\n");
            DebugP_assert(0);
        }
        testConfig.numChirpsPerFrame = atoi ((char*)&testConfig.numChirpsPerFrame);

        //CLI_write ("\nNo of frames:/>");
        status = CLI_readLine(gUartHandle[0], (char*)&testConfig.numFrames, READ_LINE_BUFSIZE);
        if(status != SystemP_SUCCESS)
        {
            CLI_write("Error reading input config\n");
            DebugP_assert(0);
        }
        testConfig.numFrames = atoi ((char*)&testConfig.numFrames);

    }

    testConfig.numTxAntennas = testConfig.numVirtualAntennas/testConfig.numRxAntennas;
    testConfig.numRangeBins = mathUtils_pow2roundup(testConfig.numAdcSamples)/2; //real only input
    testConfig.numChirpsPerFrameRef = testConfig.numChirpsPerFrame;

    numAdcSamplesPerEvt = (testConfig.numAdcSamples * testConfig.numRxAntennas);

    if ((testConfig.numTxAntennas > MAX_NUM_TX_ANTENNA) || (testConfig.numRangeBins > MAX_NUM_RANGEBIN) || (testConfig.numChirpsPerFrame > MAX_NUM_CHIRPS_PERFRAME))
    {
        CLI_write("Error: Wrong test configurations \n");
        exit(0);
    }

    //ToDo check that 4 params from ADC file match CLI configuration
    DebugP_log("numTxAntennas = %d\r", testConfig.numTxAntennas);
    DebugP_log("numRangeBins = %d\r", testConfig.numRangeBins);
    DebugP_log("numChirpsPerFrame = %d\n", testConfig.numChirpsPerFrame);
    DebugP_log("numFrames = %d\n", testConfig.numFrames);

#ifdef LOW_POWER_DEEP_SLEEP_MODE_VERIFICATION
    testConfig.numFrames = 8; //Do only 8 frames
    gPreStoredAdcTestBuff  = (int16_t *) DPC_ObjDet_MemPoolAlloc(&gMmwMssMCB.L3RamObj,
                                                                 testConfig.numAdcSamples *
                                                                 testConfig.numRxAntennas *
                                                                 testConfig.numChirpsPerFrame *
                                                                 testConfig.numTxAntennas *
                                                                 testConfig.numFrames *
                                                                 sizeof(uint16_t),
                                                                 sizeof(uint16_t));
    gStoredHeatMap   = (uint32_t *) DPC_ObjDet_MemPoolAlloc(&gMmwMssMCB.L3RamObj,
                                                           testConfig.numRangeBins *
                                                           gMmwMssMCB.sigProcChainCfg.azimuthFftSize *
                                                           testConfig.numFrames *
                                                           sizeof(uint32_t),
                                                           sizeof(uint32_t));
    numReadSamples = testConfig.numAdcSamples * testConfig.numRxAntennas * testConfig.numChirpsPerFrame * testConfig.numTxAntennas * testConfig.numFrames * sizeof(uint16_t);

    //Read numFrames into buffer:
    numReadSamples = fread(gPreStoredAdcTestBuff, sizeof(uint16_t), numReadSamples, fileIdAdcData);

#else
    if (gMmwMssMCB.adcDataSourceCfg.source == 2)
    {
        char localAdcTestBuff[READ_LINE_BUFSIZE];
        testConfig.numFrames = gMmwMssMCB.frameCfg.h_NumOfFrames; 
        if((testConfig.numFrames == 0) || (testConfig.numFrames > 8)) //Do a max of 8 frames
        {
            testConfig.numFrames = 8;
        }
        numReadSamples = testConfig.numAdcSamples * testConfig.numRxAntennas * testConfig.numChirpsPerFrame * testConfig.numTxAntennas * testConfig.numFrames * sizeof(uint16_t);

        gPreStoredAdcTestBuff  = (int16_t *) DPC_ObjDet_MemPoolAlloc(&gMmwMssMCB.L3RamObj,
                                                                    testConfig.numAdcSamples *
                                                                    testConfig.numRxAntennas *
                                                                    testConfig.numChirpsPerFrame *
                                                                    testConfig.numTxAntennas *
                                                                    testConfig.numFrames *
                                                                    sizeof(uint16_t),
                                                                    sizeof(uint16_t));

        
        //Read numFrames into buffer:
        //CLI_write("\n\nReading adc samples for all chirps and frames\n\n");
        for(frameCnt = 0; frameCnt < testConfig.numFrames; frameCnt++)
        {
            /* Read chirps from the file */
            for(i = 0; i < (testConfig.numChirpsPerFrame * testConfig.numTxAntennas); i++)
            {
                for (j = 0; j < numAdcSamplesPerEvt; j++)
                {
                    status = CLI_readLine(gUartHandle[0], (char*)localAdcTestBuff, READ_LINE_BUFSIZE);
                    if(status != SystemP_SUCCESS)
                    {
                        CLI_write("Error reading input data\n");
                        DebugP_assert(0);
                    }
                    gPreStoredAdcTestBuff[gPreStoredAdcTestBuffRdInd] = (int16_t) atoi ((char*)localAdcTestBuff);
                    gPreStoredAdcTestBuffRdInd++;
                }
            }
        }
    }
#endif

    for(frameCnt = 0; frameCnt < testConfig.numFrames; frameCnt++)
    {
#if 0
//ToDo: Remove this below, this inserts reference heatmap to test cfar dpu
        if(frameCnt >=3)
        {
            fread(detMAtrixData, sizeof(uint32_t), 32*16, fileIdDetMatDataRef);
        }
//ToDo: Remove this above
#endif
        /* Read chirps from the file */
        for(i = 0; i < (testConfig.numChirpsPerFrame * testConfig.numTxAntennas); i++)
        {
            if (!endOfFile && gMmwMssMCB.adcDataSourceCfg.source != 0)
            {
                /* Read one chirp of ADC samples and to put data in ADC test buffer */
#ifndef LOW_POWER_DEEP_SLEEP_MODE_VERIFICATION
                if (gMmwMssMCB.adcDataSourceCfg.source == 1)
                {
                numReadSamples = fread(gMmwMssMCB.adcTestBuff, sizeof(uint16_t),  numAdcSamplesPerEvt, fileIdAdcData); //ToDo Hard-coded for real samples (int16_t)
                }
                else
                {
                    numReadSamples = localRead(gMmwMssMCB.adcTestBuff, sizeof(uint16_t),  numAdcSamplesPerEvt);
                }   

#else
                numReadSamples = localRead(gMmwMssMCB.adcTestBuff, sizeof(uint16_t),  numAdcSamplesPerEvt);
#endif
                if (numReadSamples != numAdcSamplesPerEvt)
                {
                    endOfFile = true;
                }
            }

            /* Manual trigger to simulate chirp avail irq */
            errorCode = EDMAEnableTransferRegion(
                            baseAddr, regionId, EDMA_APPSS_TPCC_B_EVT_CHIRP_AVAIL_IRQ, EDMA_TRIG_MODE_MANUAL); //EDMA_TRIG_MODE_EVENT
            if (errorCode != 1)
            {
                CLI_write("Error: EDMA start Transfer returned %d\n",errorCode);
                return;
            }

            if (gMmwMssMCB.adcDataSourceCfg.source == 1)
            {
            ClockP_usleep(1000); //1ms sleep
            }

        } /* end of chirp loop */
        SemaphoreP_pend(&gMmwMssMCB.adcFileTaskSemHandle, SystemP_WAIT_FOREVER);

#ifndef LOW_POWER_DEEP_SLEEP_MODE_VERIFICATION
        
        if (gMmwMssMCB.adcDataSourceCfg.source == 1)
        {
            /* Write out Detection Matrix */
            fwrite(detMAtrixData, sizeof(uint32_t), gMmwMssMCB.sigProcChainCfg.azimuthFftSize * testConfig.numRangeBins, fileIdDetMatData);

            //Write out point cloud
            fwrite(&frameCnt, sizeof(uint16_t), 1, fileIdPointCloudIndData);
            fwrite(&result->numObjOut, sizeof(uint16_t), 1, fileIdPointCloudIndData);
            if(result->numObjOut > 0)
            {
                int objInd;
                for (objInd = 0; objInd < result->numObjOut; objInd++)
                {
                    fwrite(&gMmwMssMCB.dpcObjIndOut[objInd], sizeof(DPIF_PointCloudRngAzimElevDopInd), 1, fileIdPointCloudIndData);
                    fwrite(&gMmwMssMCB.cfarDetObjOut[objInd].snr, sizeof(uint16_t), 1, fileIdPointCloudIndData);
                    fwrite(&gMmwMssMCB.cfarDetObjOut[objInd].noise, sizeof(uint16_t), 1, fileIdPointCloudIndData);
                }
            }
        }

        printf("ADC file read task: Processed frame number %d\n", frameCnt);
#else
        if (gMmwMssMCB.adcDataSourceCfg.source == 1)
        {
            /* Write out Detection Matrix to L3 memory*/
            localWrite(detMAtrixData, sizeof(uint32_t), gMmwMssMCB.sigProcChainCfg.azimuthFftSize * testConfig.numRangeBins, fileIdDetMatData);
        }
#endif


    } /* end of frame loop */

    if (gMmwMssMCB.adcDataSourceCfg.source == 1)
    {
        fclose(fileIdAdcData);
        fclose(fileIdDetMatData);
        fclose(fileIdPointCloudIndData);
    }

    /* check the result */
    DebugP_log("Test finished!\n\r");
    DebugP_log("\n... DPC Finished, Check Output data ....  : \n\n");

}

int32_t mmwDemo_factoryCal(void)
{
    ATE_CalibData    *ateCalib = (ATE_CalibData *)&gATECalibDataStorage;
    uint16_t         calRfFreq = 0U;
    MMWave_calibCfg  factoryCalCfg = {0U};
    int32_t          retVal = SystemP_SUCCESS;
    int32_t          errCode;
    MMWave_ErrorLevel   errorLevel;
    int16_t          mmWaveErrorCode;
    int16_t          subsysErrorCode;

    /* Enable sensor boot time calibration: */
    factoryCalCfg.isFactoryCalEnabled = true;

    /*
    * @brief  FECSS RFS Boot calibration control:
    * | bits [0] | RESERVED
    * | bits [1] | VCO calibration ON/OFF control
    * | bits [2] | PD calibration ON/OFF control
    * | bits [3] | LODIST calibration ON/OFF control.
    * | bits [4] | RESERVED 
    * | bits [5] | RX IFA calibration ON/OFF control.
    * | bits [6] | RX Gain calibration ON/OFF control.
    * | bits [7] | TX power calibration ON/OFF control.
    */
    factoryCalCfg.fecRFFactoryCalCmd.h_CalCtrlBitMask = 0xCAU;
    factoryCalCfg.fecRFFactoryCalCmd.c_MiscCalCtrl = 0x0U;
    factoryCalCfg.fecRFFactoryCalCmd.c_CalRxGainSel = gMmwMssMCB.factoryCalCfg.rxGain;
    factoryCalCfg.fecRFFactoryCalCmd.c_CalTxBackOffSel[0] = gMmwMssMCB.factoryCalCfg.txBackoffSel;
    factoryCalCfg.fecRFFactoryCalCmd.c_CalTxBackOffSel[1] = gMmwMssMCB.factoryCalCfg.txBackoffSel;

#if SOC_XWRL64XX
    /* Calculate Calibrtaion Rf Frequency. */
    calRfFreq = (gMmwMssMCB.profileTimeCfg.w_ChirpRfFreqStart) + \
                ((((gMmwMssMCB.chirpSlope * 256.0)/300) * (gMmwMssMCB.profileComCfg.h_ChirpRampEndTime * 0.1)) / 2);
    factoryCalCfg.fecRFFactoryCalCmd.xh_CalRfSlope = 0x4Du; /* 2.2Mhz per uSec*/
#else
    /* Calculate Calibrtaion Rf Frequency. */
    calRfFreq = (gMmwMssMCB.profileTimeCfg.w_ChirpRfFreqStart) + \
                ((((gMmwMssMCB.chirpSlope * 256.0)/400) * (gMmwMssMCB.profileComCfg.h_ChirpRampEndTime * 0.1)) / 2);
    factoryCalCfg.fecRFFactoryCalCmd.xh_CalRfSlope = 0x3Au; /* 2.2Mhz per uSec*/
#endif

    factoryCalCfg.fecRFFactoryCalCmd.h_CalRfFreq = calRfFreq;
    factoryCalCfg.fecRFFactoryCalCmd.c_TxPwrCalTxEnaMask[0] = 0x3;
    factoryCalCfg.fecRFFactoryCalCmd.c_TxPwrCalTxEnaMask[1] = 0x1;

    /* Check if the device is RF-Trimmed */
    /* Checking on Trim is enough*/
    if(gMmwMssMCB.factoryCalCfg.atecalibinEfuse == true)
    {
        factoryCalCfg.ptrAteCalibration = NULL;
        factoryCalCfg.isATECalibEfused  = true;
    }
    else
    {
        factoryCalCfg.isATECalibEfused  = false;
        factoryCalCfg.ptrAteCalibration = (uint8_t *)&gATECalibDataStorage[4];
    }

    if(gMmwMssMCB.factoryCalCfg.restoreEnable != 0U)
    {
        if(MmwDemo_calibRestore(&gFactoryCalibDataStorage) < 0)
        {
            CLI_write ("Error: MmwDemo failed restoring calibration data from flash.\r\n");
            MmwDemo_debugAssert (0);
        }

        /* Populate calibration data pointer. */
        factoryCalCfg.ptrFactoryCalibData = &gFactoryCalibDataStorage.calibData;

        /* Disable boot calibration. */
        factoryCalCfg.isFactoryCalEnabled = false;
    }

    retVal = MMWave_factoryCalibConfig(gMmwMssMCB.ctrlHandle, &factoryCalCfg, &errCode);
    if (retVal != SystemP_SUCCESS)
    {

        /* Error: Unable to perform boot calibration */
        MMWave_decodeError (errCode, &errorLevel, &mmWaveErrorCode, &subsysErrorCode);

        /* Error: Unable to initialize the mmWave control module */
        CLI_write ("Error: mmWave Control Initialization failed [Error code %d] [errorLevel %d] [mmWaveErrorCode %d] [subsysErrorCode %d]\n", errCode, errorLevel, mmWaveErrorCode, subsysErrorCode);
        if (mmWaveErrorCode == MMWAVE_ERFSBOOTCAL)
        {
            CLI_write ("Error: Boot Calibration failure\n");
            ateCalib->validityFlag = 0x0U; /* Flag to indicate to re-run ATE calibration */
        }
        else
        {
            MmwDemo_debugAssert (0);
        }
    }

    /* Save calibration data in flash */
    if(gMmwMssMCB.factoryCalCfg.saveEnable != 0)
    {
        gFactoryCalibDataStorage.magic = MMWDEMO_CALIB_STORE_MAGIC;
        retVal = rl_fecssRfFactoryCalDataGet(M_DFP_DEVICE_INDEX_0, &gFactoryCalibDataStorage.calibData);
        if(retVal != M_DFP_RET_CODE_OK)
        {
            /* Error: Calibration data restore failed */
            CLI_write("Error: MMW demo failed rl_fecssRfFactoryCalDataGet with Error[%d]\n", retVal);
            retVal = SystemP_FAILURE;
        }

        /* Save data in flash */
        retVal = MmwDemo_calibSave(&gFactoryCalibDataStorage);
        if(retVal < 0)
        {
            CLI_write("Error: MMW demo failed Calibration Save with Error[%d]\n", retVal);
            MmwDemo_debugAssert (0);
        }
    }
#ifdef SOC_XWRL14XX
    /* Configuring command for Run time CLPC calibration */
    gMmwMssMCB.fecTxclpcCalCmd.c_CalMode = 0x0u; //Run Tx CLPC and apply bias codes to hardware
    gMmwMssMCB.fecTxclpcCalCmd.c_CalTxBackOffSel[0] = factoryCalCfg.fecRFFactoryCalCmd.c_CalTxBackOffSel[0];
    gMmwMssMCB.fecTxclpcCalCmd.c_CalTxBackOffSel[1] = factoryCalCfg.fecRFFactoryCalCmd.c_CalTxBackOffSel[1];
    gMmwMssMCB.fecTxclpcCalCmd.h_CalRfFreq = factoryCalCfg.fecRFFactoryCalCmd.h_CalRfFreq;
    gMmwMssMCB.fecTxclpcCalCmd.xh_CalRfSlope = factoryCalCfg.fecRFFactoryCalCmd.xh_CalRfSlope;
    gMmwMssMCB.fecTxclpcCalCmd.c_TxPwrCalTxEnaMask[0] = factoryCalCfg.fecRFFactoryCalCmd.c_TxPwrCalTxEnaMask[0];
    gMmwMssMCB.fecTxclpcCalCmd.c_TxPwrCalTxEnaMask[1] = factoryCalCfg.fecRFFactoryCalCmd.c_TxPwrCalTxEnaMask[1];
#endif

    return retVal;
}

int32_t mmwDemo_mmWaveInit(bool iswarmstrt)
{
    int32_t             errCode;
    int32_t             retVal = SystemP_SUCCESS;
    MMWave_InitCfg      initCfg;
    MMWave_ErrorLevel   errorLevel;
    int16_t             mmWaveErrorCode;
    int16_t             subsysErrorCode;

    /* Initialize the mmWave control init configuration */
    memset ((void*)&initCfg, 0, sizeof(MMWave_InitCfg));

    /* Is Warm Start? */
    initCfg.iswarmstart = iswarmstrt;

    /* Initialize and setup the mmWave Control module */
    gMmwMssMCB.ctrlHandle = MMWave_init (&initCfg, &errCode);
    if (gMmwMssMCB.ctrlHandle == NULL)
    {
        /* Error: Unable to initialize the mmWave control module */
        MMWave_decodeError (errCode, &errorLevel, &mmWaveErrorCode, &subsysErrorCode);

        /* Error: Unable to initialize the mmWave control module */
        CLI_write ("Error: mmWave Control Initialization failed [Error code %d] [errorLevel %d] [mmWaveErrorCode %d] [subsysErrorCode %d]\n", errCode, errorLevel, mmWaveErrorCode, subsysErrorCode);
        retVal = SystemP_FAILURE;
    }
    /* FECSS RF Power ON*/
    if(initCfg.iswarmstart)
    {
        errCode = rl_fecssRfPwrOnOff(M_DFP_DEVICE_INDEX_0, &gMmwMssMCB.channelCfg);
        if(errCode != M_DFP_RET_CODE_OK)
        {
            CLI_write ("Error: FECSS RF PowerON failed [Error code %d] \r\n", errCode);
            retVal = SystemP_FAILURE;
        }   
    }

    return retVal;
}

/* Enable Continuous wave mode */
#define CONTINUOS_WAVE_MODE_ENABLE   0

/**
 *  @b Description
 *  @n
 *      mmw demo helper Function to start sensor.
 *
 *  @retval
 *      Success     - 0
 *  @retval
 *      Error       - <0
 */
int32_t MmwDemo_startSensor(void)
{
#if !(CONTINUOS_WAVE_MODE_ENABLE) /* suppress warning */
    int32_t     errCode;
#endif
    MMWave_CalibrationCfg   calibrationCfg;


    /*****************************************************************************
     * RF :: now start the RF and the real time ticking
     *****************************************************************************/
    /* Initialize the calibration configuration: */
    memset ((void *)&calibrationCfg, 0, sizeof(MMWave_CalibrationCfg));
    /* Populate the calibration configuration: */
    Mmwave_populateDefaultCalibrationCfg (&calibrationCfg);

    DebugP_logInfo("App: MMWave_start Issued\n");

#if !(CONTINUOS_WAVE_MODE_ENABLE) /* disable mmWave_start for continousMode CW */
    /* Start the mmWave module: The configuration has been applied successfully. */
    if (MMWave_start(gMmwMssMCB.ctrlHandle, &calibrationCfg, &gMmwMssMCB.sensorStart, &errCode) < 0)
    {
        MMWave_ErrorLevel   errorLevel;
        int16_t             mmWaveErrorCode;
        int16_t             subsysErrorCode;

        /* Error/Warning: Unable to start the mmWave module */
        MMWave_decodeError (errCode, &errorLevel, &mmWaveErrorCode, &subsysErrorCode);
        CLI_write ("Error: mmWave Start failed [mmWave Error: %d Subsys: %d]\n", mmWaveErrorCode, subsysErrorCode);
        /* datapath has already been moved to start state; so either we initiate a cleanup of start sequence or
           assert here and re-start from the beginning. For now, choosing the latter path */
        MmwDemo_debugAssert(0);
        return -1;
    }
#endif
    return 0;
}

/**
 *  @b Description
 *  @n
 *      mmw demo helper Function to do one time sensor initialization.
 *      User need to fill gMmwMssMCB.mmwOpenCfg before calling this function
 *
 *  @retval
 *      Success     - 0
 *  @retval
 *      Error       - <0
 */
int32_t MmwDemo_openSensor(void)
{
    int32_t             errCode;
    MMWave_ErrorLevel   errorLevel;
    int16_t             mmWaveErrorCode;
    int16_t             subsysErrorCode;

    /**********************************************************
     **********************************************************/

    /* Open mmWave module, this is only done once */

    /* Open the mmWave module: */
    if (MMWave_open (gMmwMssMCB.ctrlHandle, &gMmwMssMCB.mmwOpenCfg, &errCode) < 0)
    {
        /* Error: decode and Report the error */
        MMWave_decodeError (errCode, &errorLevel, &mmWaveErrorCode, &subsysErrorCode);
        CLI_write ("Error: mmWave Open failed [Error code: %d Subsystem: %d]\n",
                        mmWaveErrorCode, subsysErrorCode);
        return -1;
    }

    return 0;
}

/**
 *  @b Description
 *  @n
 *      MMW demo helper Function to configure sensor. User need to fill gMmwMssMCB.mmwCtrlCfg and
 *      add profiles/chirp to mmWave before calling this function
 *
 *  @retval
 *      Success     - 0
 *  @retval
 *      Error       - <0
 */
int32_t MmwDemo_configSensor(void)
{
    int32_t     errCode = 0;

    /* Configure the mmWave module: */
    if (MMWave_config (gMmwMssMCB.ctrlHandle, &gMmwMssMCB.mmwCtrlCfg, &errCode) < 0)
    {
        MMWave_ErrorLevel   errorLevel;
        int16_t             mmWaveErrorCode;
        int16_t             subsysErrorCode;

        /* Error: Report the error */
        MMWave_decodeError (errCode, &errorLevel, &mmWaveErrorCode, &subsysErrorCode);
        CLI_write ("Error: mmWave Config failed [Error code: %d Subsystem: %d]\n",
                        mmWaveErrorCode, subsysErrorCode);
        goto exit;
    }

exit:
    return errCode;
}

#if 0
int32_t mmwDemo_registerBurstInterrupt(void)
{
    int32_t           regVal, retVal = 0;
    int32_t           status = SystemP_SUCCESS;
    HwiP_Params       hwiPrms;

    // Configure the interrupt for Burst End
    regVal = HW_RD_REG32(CSL_APP_CTRL_U_BASE + CSL_APP_CTRL_APPSS_IRQ_REQ_SEL);
    regVal = regVal | 0x1000;
    HW_WR_REG32((CSL_APP_CTRL_U_BASE + CSL_APP_CTRL_APPSS_IRQ_REQ_SEL), regVal);

    /* Register interrupt */
    HwiP_Params_init(&hwiPrms);
    hwiPrms.intNum      = 16 + CSL_APPSS_INTR_MUXED_FECSS_CHIRPTIMER_BURST_START_AND_BURST_END;
    hwiPrms.callback    = mmwDemoBurstISR;
    //hwiPrms.priority    = 0;
    hwiPrms.args        = NULL;
    status              = HwiP_construct(&gHwiChirpAvailableHwiObject, &hwiPrms);

    if(SystemP_SUCCESS != status)
    {
        retVal = SystemP_FAILURE;
    }
    else
    {
        HwiP_enableInt((uint32_t)CSL_APPSS_INTR_MUXED_FECSS_CHIRPTIMER_BURST_START_AND_BURST_END);
    }

    return retVal;
}

int32_t mmwDemo_registerChirpInterrupt(void)
{
    int32_t           retVal = 0;
    int32_t           status = SystemP_SUCCESS;
    HwiP_Params       hwiPrms;

    /* Register interrupt */
    HwiP_Params_init(&hwiPrms);
    hwiPrms.intNum      = 16 + CSL_APPSS_INTR_MUXED_FECSS_CHIRPTIMER_CHIRP_START_AND_CHIRP_END;
    hwiPrms.callback    = mmwDemoChirpStartISR;
    //hwiPrms.priority    = 0;
    hwiPrms.args        = NULL;
    status              = HwiP_construct(&gHwiChirpAvailableHwiObject, &hwiPrms);

    if(SystemP_SUCCESS != status)
    {
        retVal = SystemP_FAILURE;
    }
    else
    {
        HwiP_enableInt((uint32_t)CSL_APPSS_INTR_MUXED_FECSS_CHIRPTIMER_CHIRP_START_AND_CHIRP_END);
    }

    return retVal;
}

int32_t mmwDemo_registerChirpAvailableInterrupts(void)
{
    int32_t           retVal = 0;
    int32_t           status = SystemP_SUCCESS;
    HwiP_Params       hwiPrms;


    /* Register interrupt */
    HwiP_Params_init(&hwiPrms);
    hwiPrms.intNum      = 16 + CSL_APPSS_INTR_MUXED_FECSS_CHIRP_AVAIL_IRQ_AND_ADC_VALID_START_AND_SYNC_IN; //CSL_MSS_INTR_RSS_ADC_CAPTURE_COMPLETE;
    hwiPrms.callback    = mmwDemoChirpISR;
    //hwiPrms.priority    = 0;
    hwiPrms.args        = NULL;
    status              = HwiP_construct(&gHwiChirpAvailableHwiObject, &hwiPrms);

    if(SystemP_SUCCESS != status)
    {
        retVal = SystemP_FAILURE;
    }
    else
    {
        HwiP_enableInt((uint32_t)CSL_APPSS_INTR_MUXED_FECSS_CHIRP_AVAIL_IRQ_AND_ADC_VALID_START_AND_SYNC_IN);
    }

    return retVal;
}
#endif

int32_t mmwDemo_registerFrameStartInterrupt(void)
{
    int32_t           retVal = 0;
    int32_t           status = SystemP_SUCCESS;
    HwiP_Params       hwiPrms;


    /* Register interrupt */
    HwiP_Params_init(&hwiPrms);
    hwiPrms.intNum      = 16 + CSL_APPSS_INTR_FECSS_FRAMETIMER_FRAME_START;
    hwiPrms.callback    = mmwDemoFrameStartISR;
    //hwiPrms.priority    = 0;
    hwiPrms.args        = (void *) &gMmwMssMCB;
    status              = HwiP_construct(&gHwiFrameStartHwiObject, &hwiPrms);

    if(SystemP_SUCCESS != status)
    {
        retVal = SystemP_FAILURE;
    }
    else
    {
        HwiP_enableInt((uint32_t)CSL_APPSS_INTR_FECSS_FRAMETIMER_FRAME_START);
    }

    return retVal;
}

void motion_detect(void* args)
{
    int32_t errorCode = SystemP_SUCCESS;

    /* Peripheral Driver Initialization */
    Drivers_open();
    Board_driversOpen();

    // Configure the LED GPIO
    gpioBaseAddrLed = (uint32_t) AddrTranslateP_getLocalAddr(GPIO_LED_BASE_ADDR);
    pinNumLed       = GPIO_LED_PIN;
    GPIO_setDirMode(gpioBaseAddrLed, pinNumLed, GPIO_LED_DIR);

    /* HWASS_SHRD_RAM0, HWASS_SHRD_RAM1, TPCC0 and TPCC1 memory Initialization. */
    SOC_memoryInit();

    gMmwMssMCB.commandUartHandle = gUartHandle[0];

    /* mmWave initialization*/
    mmwDemo_mmWaveInit(0);

    /* Initialize default antenna geometry */
    memcpy((void *) &gMmwMssMCB.antennaGeometryCfg, (void *) &gDefaultAntGeometry, sizeof(MmwDemo_antennaGeometryCfg));

    // RPMF: Create a Task for Power Management Framework
    gPowerTask = xTaskCreateStatic( powerManagementTask,      /* Pointer to the function that implements the task. */
                                  "power",          /* Text name for the task.  This is to facilitate debugging only. */
                                  POWER_TASK_SIZE,  /* Stack depth in units of StackType_t typically uint32_t on 32b CPUs */
                                  NULL,            /* We are not using the task parameter. */
                                  POWER_TASK_PRI,   /* task priority, 0 is lowest priority, configMAX_PRIORITIES-1 is highest */
                                  gPowerTaskStack,  /* pointer to stack base */
                                  &gPowerTaskObj ); /* pointer to statically allocated task object memory */
                                  
    //RPMF: Create Semaphore for to pend Power Task
    gPowerSem = xSemaphoreCreateBinaryStatic(&gPowerSemObj);

    /* Create binary semaphore to pend Main task, */
    SemaphoreP_constructBinary(&gMmwMssMCB.demoInitTaskCompleteSemHandle, 0);

    errorCode = SemaphoreP_constructBinary(&gMmwMssMCB.cliInitTaskCompleteSemHandle, 0);
    DebugP_assert(SystemP_SUCCESS == errorCode);

    errorCode = SemaphoreP_constructBinary(&gMmwMssMCB.TestSemHandle, 0);
    DebugP_assert(SystemP_SUCCESS == errorCode);

    errorCode = SemaphoreP_constructBinary(&gMmwMssMCB.tlvSemHandle, 0);
    DebugP_assert(SystemP_SUCCESS == errorCode);

    errorCode = SemaphoreP_constructBinary(&gMmwMssMCB.adcFileTaskSemHandle, 0);
    DebugP_assert(SystemP_SUCCESS == errorCode);

    /* Re-trive ATE-Calibration from Flash Offset: 0x80000 */
    if(MmwDemo_calibInit() < 0)
    {
        CLI_write("Error: ATE Calibration data initialization failed \n");
        MmwDemo_debugAssert (0);
    }

    /* DPC initialization*/
    DPC_Init();

    // Make the LED HIGH
    GPIO_pinWriteHigh(gpioBaseAddrLed, pinNumLed);

    CLI_init(CLI_TASK_PRIORITY);

    /* Never return for this task. */
    SemaphoreP_pend(&gMmwMssMCB.demoInitTaskCompleteSemHandle, SystemP_WAIT_FOREVER);

    Board_driversClose();
    Drivers_close();
}

//Idle3 Handle;
int power_idleresumehook(uint_fast16_t eventType, uintptr_t eventArg, uintptr_t clientArg)
{
    return Power_NOTIFYDONE;
}


void power_LPDSentryhook(void)
{
    // Anything to do before LPDS entry
}


void power_LPDSresumehook(void)
{
    static uint8_t ledState = 0;
    // Re-Init MPU
    MpuP_init();

    // Debug log init
    DebugP_logZoneEnable(DebugP_LOG_ZONE_ERROR);
    DebugP_logZoneEnable(DebugP_LOG_ZONE_WARN);

    // Initialize Clock
    ClockP_init();
    // Initialize timer
    TimerP_init();
    PowerClock_init();

    // Now we can do pinmux
    Pinmux_init();

    // Re-initialize all peripheral drivers
    QSPI_init();
    EDMA_init();
    HWA_init();
    I2C_init();
    Power_init();
    Drivers_uartInit();
    Drivers_open();
    Board_driversOpen();
    // Toggle the LED indicating out of deep sleep
    if(ledState == 0)
    {
        ledState = 1;
        GPIO_pinWriteLow(gpioBaseAddrLed, pinNumLed);
    }
    else
    {
       ledState = 0;
       GPIO_pinWriteHigh(gpioBaseAddrLed, pinNumLed);
    }
}

#ifdef LOW_POWER_DEEP_SLEEP_MODE_VERIFICATION
/* ToDo This is for debugging, set to one to stop the code after power up, and reconnect CCS*/
volatile int gDebugLowPowerModeBreakFlag = 0;
#endif

/* RPMF: Power Management Task */
void powerManagementTask(void *args)
{
    while(1)
    {
        /* Wait till the UART transmit is complete. Once UART data (if any) is transmitted, get to low power state */
        xSemaphoreTake(gPowerSem, portMAX_DELAY);

        /* Delete the DPC task : We are recreating this task during exit of Low Power mode */
        vTaskDelete(gDpcTask);
        /* Delete the UART Tx task : We are recreating this task during exit of Low Power mode*/
        vTaskDelete(gTlvTask);
        /* Delete the CLI task */
        if (gCliTask != NULL)
        {
            vTaskDelete(gCliTask);
            gCliTask = NULL;
        }
        /* Get the Frame Periodicity*/
        frmPrdus = (gMmwMssMCB.frameCfg.w_FramePeriodicity * 0.025);
        /* Idle time remaining for Low Power State */
        slpTimeus = (unsigned long long)(frmPrdus - demoTimeus);
        /*If low power mode is enabled, call the Power Framework with frame idle time */
        if(gMmwMssMCB.lowPowerMode == LOW_PWR_MODE_ENABLE)
        {
            /* RPMF driver call for getting to Low Power State */
            Power_idleFunc(slpTimeus);
            Power_disablePolicy();
            /* Based on idle time left, different low power modes like LPDS, Idle can be taken */
            demoLowPwrStateTaken = Power_getLowPowModeTaken();
            if(demoLowPwrStateTaken == POWER_NONE)
            {
                /* Use Low Power config only when there is sufficient time. */
                CLI_write("Error: No Sufficient Time for getting into Low Power Modes.\n"); 
                DebugP_assert(0);
            }
        }
        /* In Low Power Test mode, actual Low Power mode is not taken */
        if(gMmwMssMCB.lowPowerMode == LOW_PWR_TEST_MODE)
        {
            /* Wait till the frame period expires */
            uint64_t frmPeriod = (uint32_t) (frmPrdus * 32.768e-3);
            while ((gMmwMssMCB.frameStartTimeStampSlowClk + frmPeriod) > PRCMSlowClkCtrGet())
            {
            }
            power_LPDSresumehook();
        }

        /* Enable MDLL Clock if ADC logging is enabled */
        if(gMmwMssMCB.adcLogging == true)
        {
            /* Enable MDLL Clock for RDIF interface */
            SOC_enableMDLLClock();
        }

        /* If Idle3 is low power mode taken, re-initialize the EDMA */
        if(demoLowPwrStateTaken == POWER_IDLE)
        {
            static volatile uint32_t ledCnt = 0,ledStateIdle = 0;
            // Free up all the edma channels and close the EDMA interface 
            mmwDemo_freeDmaChannels(gEdmaHandle[0]);
            Drivers_edmaClose();
            EDMA_deinit();
            // Re-init the EDMA interface for next configuration
            EDMA_init();
            Drivers_edmaOpen();
            if((ledCnt%8) == 0)
            {
                // Toggle the LED indicating out of Idle
                if(ledStateIdle == 0)
                {
                    ledStateIdle = 1;
                    GPIO_pinWriteLow(gpioBaseAddrLed, pinNumLed);
                }
                else
                {
                    ledStateIdle = 0;
                    GPIO_pinWriteHigh(gpioBaseAddrLed, pinNumLed);
                }
            }
            ledCnt++;
        }

        /* DPC initialization */
        DPC_Init();

        /* mmWave initialization as Warmstart */
        mmwReinitStatus = mmwDemo_mmWaveInit(1);

#ifdef LOW_POWER_DEEP_SLEEP_MODE_VERIFICATION
        /* This loop is for debugging */
        while(gDebugLowPowerModeBreakFlag)
        {
        }
        if((gMmwMssMCB.adcDataSourceCfg.source == 1) && (gMmwMssMCB.lowPowerMode == LOW_PWR_MODE_ENABLE))
        {
            gDebugLowPowerModeBreakFlag = 1;
        }
#endif
        /*If finite frames are configured, stop the demo after configured frames are trasnmitted */
        if((gMmwMssMCB.frameCfg.h_NumOfFrames != 0) && \
                (gMmwMssMCB.frameCfg.h_NumOfFrames == gMmwMssMCB.stats.frameStartIntCounter))
        {
            int32_t err;
            // Delete the exisitng profile as we receive a new configuration
            MMWave_delProfile(gMmwMssMCB.ctrlHandle,gMmwMssMCB.mmwCtrlCfg.frameCfg[0].profileHandle[0],&err);
            // Demo Stopped
            rangeProcHWAObj* temp = gMmwMssMCB.rangeProcDpuHandle;
            temp->inProgress = false;
            gMmwMssMCB.oneTimeConfigDone = 0;
            gMmwMssMCB.stats.frameStartIntCounter = 0;
            sensorStop = 0;
            isSensorStarted = 0;
            /* Restart the CLI task */
            CLI_init(CLI_TASK_PRIORITY);
        }
        else
        {
            /* Continue Next frame */
            CLI_MMWStart();

            if(gMmwMssMCB.adcDataSourceCfg.source == 1 || gMmwMssMCB.adcDataSourceCfg.source == 2)
            {
                /* In test mode trigger next frame processing */
                SemaphoreP_post(&gMmwMssMCB.adcFileTaskSemHandle);
            }
        }
    }
}

// Free all the allocated EDMA channels
static void mmwDemo_freeDmaChannels(EDMA_Handle edmaHandle)
{
    uint32_t   index;
    uint32_t  dmaCh, tcc, pram, shadow;
    for(index = 0; index < 64; index++)
    {
        dmaCh = index;
        tcc = index;
        pram = index;
        shadow = index;
        DPEDMA_freeEDMAChannel(edmaHandle, &dmaCh, &tcc, &pram, &shadow);
    }
    for(index = 0; index < 128; index++)
    {
        shadow = index;
        DebugP_assert(EDMA_freeParam(edmaHandle, &shadow) == SystemP_SUCCESS);
    }
    return;
}
